From f7292b66a66d5925d4e2cd5a1025a16cf26052a2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 30 Jan 2025 10:06:07 +0900 Subject: [PATCH] =?UTF-8?q?Visualizer=E3=81=AB=E9=80=81=E4=BF=A1=E3=81=99?= =?UTF-8?q?=E3=82=8B=E6=8F=8F=E7=94=BB=E5=91=BD=E4=BB=A4=E3=82=92SVG?= =?UTF-8?q?=E3=81=AB=E5=A4=89=E6=9B=B4=E3=81=99=E3=82=8B=20(#711)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .github/workflows/custom_dict.json | 4 +- consai_ros2/robocup_ssl_msgs/CMakeLists.txt | 8 +- crane_bringup/launch/crane.launch.py | 53 +- crane_bringup/package.xml | 1 + .../crane_game_analyzer/game_analyzer.hpp | 4 +- .../kick_event_detector.hpp | 44 +- .../src/crane_game_analyzer.cpp | 6 +- crane_local_planner/CMakeLists.txt | 2 +- .../crane_local_planner/gridmap_planner.hpp | 2 +- .../crane_local_planner/local_planner.hpp | 2 +- .../crane_local_planner/planner_base.hpp | 6 +- .../crane_local_planner/simple_planner.hpp | 17 +- .../src/crane_local_planner.cpp | 81 +- crane_msgs/CMakeLists.txt | 2 +- crane_msgs/msg/analysis/PlaySituation.msg | 2 + crane_msgs/package.xml | 1 + crane_play_switcher/src/play_switcher.cpp | 1 + crane_robot_receiver/src/ping_status_node.cpp | 2 +- .../src/robot_receiver_node.cpp | 10 +- .../include/crane_robot_skills/skill_base.hpp | 8 +- crane_robot_skills/src/attacker.cpp | 15 +- crane_robot_skills/src/goal_kick.cpp | 5 +- crane_robot_skills/src/goalie.cpp | 13 +- crane_robot_skills/src/kick.cpp | 59 +- crane_robot_skills/src/marker.cpp | 15 +- crane_robot_skills/src/penalty_kick.cpp | 4 +- crane_robot_skills/src/receive.cpp | 10 +- .../src/single_ball_placement.cpp | 72 +- crane_robot_skills/src/sub_attacker.cpp | 46 +- crane_simple_ai/include/crane_commander.hpp | 8 +- .../proto/ObjectsArray.proto | 138 +-- .../visualization_data_handler.hpp | 9 +- .../world_model_publisher.hpp | 4 +- .../src/visualization_data_handler.cpp | 492 +++++----- .../src/world_model_publisher.cpp | 41 +- .../attacker_skill_planner.hpp | 32 +- .../offensive_planner.hpp | 32 +- .../placement_avoidance_planner.hpp | 8 +- .../crane_planner_plugins/planner_base.hpp | 6 +- .../simple_placer_planner.hpp | 24 +- .../session_controller.hpp | 2 +- .../src/crane_session_controller.cpp | 4 +- .../consai_visualizer_wrapper.hpp | 903 ------------------ .../crane_visualizer_wrapper.hpp | 636 ++++++++++++ .../.clang-format | 17 + .../CMakeLists.txt | 24 + .../package.xml | 23 + .../crane_visualization_aggregator_node.cpp | 62 ++ 48 files changed, 1512 insertions(+), 1448 deletions(-) delete mode 100644 utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp create mode 100644 utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp create mode 100755 utility/crane_visualization_aggregator/.clang-format create mode 100755 utility/crane_visualization_aggregator/CMakeLists.txt create mode 100755 utility/crane_visualization_aggregator/package.xml create mode 100644 utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp diff --git a/.github/workflows/custom_dict.json b/.github/workflows/custom_dict.json index bbb709f39..91ee44063 100644 --- a/.github/workflows/custom_dict.json +++ b/.github/workflows/custom_dict.json @@ -125,6 +125,8 @@ "libprotobuf", "RELWITHDEBINFO", "NODEBUG", - "remmina" + "remmina", + "gltf", + "svgs" ] } diff --git a/consai_ros2/robocup_ssl_msgs/CMakeLists.txt b/consai_ros2/robocup_ssl_msgs/CMakeLists.txt index d8f47de27..50f87b1f8 100644 --- a/consai_ros2/robocup_ssl_msgs/CMakeLists.txt +++ b/consai_ros2/robocup_ssl_msgs/CMakeLists.txt @@ -47,15 +47,9 @@ add_library(proto_cpp target_link_libraries(proto_cpp ${PROTOBUF_LIBRARIES}) # Install -if("$ENV{ROS_DISTRO}" STRGREATER "galactic") - install( - FILES ${PROTO_H} - DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME}) -else() install( FILES ${PROTO_H} - DESTINATION include/${PROJECT_NAME}) -endif() + DESTINATION include/${PROJECT_NAME}/${PROJECT_NAME}) install( TARGETS proto_cpp diff --git a/crane_bringup/launch/crane.launch.py b/crane_bringup/launch/crane.launch.py index 4aa023bf1..76aada2c2 100644 --- a/crane_bringup/launch/crane.launch.py +++ b/crane_bringup/launch/crane.launch.py @@ -58,9 +58,6 @@ def generate_launch_description(): DeclareLaunchArgument( "max_vel", default_value="3.0", description="ロボットの最大速度" ), - DeclareLaunchArgument( - "gui", default_value="true", description="consai_visualizerの起動フラグ" - ), DeclareLaunchArgument( "speak", default_value="true", description="音声ノードの起動フラグ" ), @@ -215,6 +212,11 @@ def generate_launch_description(): # parameters=[{"blue_port": 10311}, {"yellow_port": 10312}], parameters=[{"blue_port": 10301}, {"yellow_port": 10302}], ), + Node( + package="crane_visualization_aggregator", + executable="crane_visualization_aggregator_node", + output="screen", + ), Node( package="crane_world_model_publisher", executable="crane_world_model_publisher_node", @@ -263,12 +265,6 @@ def generate_launch_description(): {"voicevox_plugin/volumeScale": 1.0}, ], ), - Node( - condition=IfCondition(LaunchConfiguration("gui")), - package="consai_visualizer", - executable="consai_visualizer", - on_exit=default_exit_behavior, - ), # rosbag recordの起動設定 GroupAction( condition=IfCondition(LaunchConfiguration("record")), @@ -279,5 +275,44 @@ def generate_launch_description(): ), ], ), + # https://github.com/foxglove/ros-foxglove-bridge/blob/main/ros2_foxglove_bridge/launch/foxglove_bridge_launch.xml + Node( + package="foxglove_bridge", + executable="foxglove_bridge", + parameters=[ + {"port": 8765}, + {"address": "0.0.0.0"}, + {"tls": False}, + {"certfile": ""}, + {"keyfile": ""}, + {"topic_whitelist": [".*"]}, + {"service_whitelist": [".*"]}, + {"param_whitelist": [".*"]}, + {"client_topic_whitelist": [".*"]}, + {"min_qos_depth": 1}, + {"max_qos_depth": 10}, + {"num_threads": 0}, + {"send_buffer_limit": 10000000}, + {"use_sim_time": False}, + { + "capabilities": [ + "clientPublish", + "parameters", + "parametersSubscribe", + "services", + "connectionGraph", + "assets", + ] + }, + {"include_hidden": False}, + { + "asset_uri_allowlist": [ + "^package://(?:\\w+/)*\\w+\\.(?:dae|fbx|glb|gltf|jpeg|jpg|mtl|obj|png|stl|tif|tiff|urdf|webp|xacro)$" + ] + }, + ], + output="screen", + on_exit=default_exit_behavior, + ), ] ) diff --git a/crane_bringup/package.xml b/crane_bringup/package.xml index 2c8f271e2..9c5263434 100755 --- a/crane_bringup/package.xml +++ b/crane_bringup/package.xml @@ -9,6 +9,7 @@ ament_cmake_auto ament_index_python + foxglove_bridge joy launch_ros diff --git a/crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp b/crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp index 94b28cd07..776c908af 100644 --- a/crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp +++ b/crane_game_analyzer/include/crane_game_analyzer/game_analyzer.hpp @@ -8,7 +8,7 @@ #define CRANE_GAME_ANALYZER__GAME_ANALYZER_HPP_ #include -#include +#include #include #include #include @@ -126,7 +126,7 @@ class GameAnalyzerComponent : public rclcpp::Node GameAnalyzerConfig config; - ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; }; } // namespace crane diff --git a/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp b/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp index 2eae34b28..bc0e50810 100644 --- a/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp +++ b/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp @@ -32,7 +32,7 @@ class KickEventDetector void update( const WorldModelWrapper & world_model, - const ConsaiVisualizerBuffer::MessageBuilder::UniquePtr & visualizer) + const CraneVisualizerBuffer::MessageBuilder::UniquePtr & visualizer) { { Record record; @@ -59,14 +59,28 @@ class KickEventDetector std::optional kick_event_origin = std::nullopt; for (const auto & id : detected_bots.friends) { RCLCPP_INFO_STREAM(rclcpp::get_logger("aaaa"), "Detected friend: " << static_cast(id)); - visualizer->addCircle( - world_model.getOurRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK"); + SvgCircleBuilder circle_builder; + circle_builder.center(world_model.getOurRobot(id)->pose.pos) + .radius(0.5) + .stroke("blue") + .fill("blue") + .strokeWidth(2); + visualizer->add(circle_builder.getSvgString()); + // visualizer->addCircle( + // world_model.getOurRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK"); kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{true, id}); } for (const auto & id : detected_bots.enemies) { RCLCPP_INFO_STREAM(rclcpp::get_logger("aaaa"), "Detected enemy: " << static_cast(id)); - visualizer->addCircle( - world_model.getTheirRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK"); + SvgCircleBuilder circle_builder; + circle_builder.center(world_model.getTheirRobot(id)->pose.pos) + .radius(0.5) + .stroke("blue") + .fill("blue") + .strokeWidth(2); + visualizer->add(circle_builder.getSvgString()); + // visualizer->addCircle( + // world_model.getTheirRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK"); kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{false, id}); } @@ -78,14 +92,28 @@ class KickEventDetector // キック中断判定 kick_history.emplace_back(ongoing_kick_origin.value(), world_model.ball.pos); ongoing_kick_origin = std::nullopt; - visualizer->addCircle(world_model.ball.pos, 3.5, 2, "green", "black", 1.0, "EVENT"); + SvgCircleBuilder circle_builder; + circle_builder.center(world_model.ball.pos) + .radius(3.5) + .stroke("green") + .fill("black") + .strokeWidth(2); + visualizer->add(circle_builder.getSvgString()); + // visualizer->addCircle(world_model.ball.pos, 3.5, 2, "green", "black", 1.0, "EVENT"); } } // 進行中のキックを可視化 if (ongoing_kick_origin.has_value()) { - visualizer->addTube( - world_model.ball.pos, ongoing_kick_origin.value().position, 0.2, 2, "red", "", 1.0, "KICK"); + SvgLineBuilder line_builder; + line_builder.start(ongoing_kick_origin.value().position) + .end(world_model.ball.pos) + .stroke("red") + .strokeWidth(2); + visualizer->add(line_builder.getSvgString()); + // visualizer->addTube( + // world_model.ball.pos, ongoing_kick_origin.value().position, + // 0.2, 2, "red", "", 1.0, "KICK"); } // キックの履歴を可視化 diff --git a/crane_game_analyzer/src/crane_game_analyzer.cpp b/crane_game_analyzer/src/crane_game_analyzer.cpp index 29fa38aab..f7ad8a639 100644 --- a/crane_game_analyzer/src/crane_game_analyzer.cpp +++ b/crane_game_analyzer/src/crane_game_analyzer.cpp @@ -15,11 +15,11 @@ namespace crane { GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options) : Node("crane_game_analyzer", options), - visualizer(std::make_unique("game_analyzer")) + visualizer(std::make_unique("game_analyzer")) { RCLCPP_INFO(get_logger(), "GameAnalyzer is constructed."); - ConsaiVisualizerBuffer::activate(*this); + CraneVisualizerBuffer::activate(*this); world_model = std::make_unique(*this); @@ -37,7 +37,7 @@ GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options robot_collision_info->relative_velocity); } visualizer->flush(); - ConsaiVisualizerBuffer::publish(); + CraneVisualizerBuffer::publish(); }); } diff --git a/crane_local_planner/CMakeLists.txt b/crane_local_planner/CMakeLists.txt index cf6509d18..ee7ba6e6a 100755 --- a/crane_local_planner/CMakeLists.txt +++ b/crane_local_planner/CMakeLists.txt @@ -29,7 +29,7 @@ target_precompile_headers(${PROJECT_NAME}_component "" "" "" - "" + "" "" ) diff --git a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp index f9a8c8d2c..633e67a7b 100644 --- a/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/gridmap_planner.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp index 7f9b7b635..4c4e62997 100644 --- a/crane_local_planner/include/crane_local_planner/local_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/local_planner.hpp @@ -40,7 +40,7 @@ class LocalPlannerComponent : public rclcpp::Node declare_parameter("planner", "rvo2"); auto planner_str = get_parameter("planner").as_string(); - crane::ConsaiVisualizerBuffer::activate(*this); + crane::CraneVisualizerBuffer::activate(*this); process_time_pub = create_publisher("process_time", 10); // if (planner_str == "gridmap") { diff --git a/crane_local_planner/include/crane_local_planner/planner_base.hpp b/crane_local_planner/include/crane_local_planner/planner_base.hpp index 9494a3e1e..04b4175a9 100644 --- a/crane_local_planner/include/crane_local_planner/planner_base.hpp +++ b/crane_local_planner/include/crane_local_planner/planner_base.hpp @@ -7,7 +7,7 @@ #ifndef CRANE_LOCAL_PLANNER__PLANNER_BASE_HPP_ #define CRANE_LOCAL_PLANNER__PLANNER_BASE_HPP_ -#include +#include #include #include #include @@ -18,14 +18,14 @@ class LocalPlannerBase { public: LocalPlannerBase(const std::string & name, rclcpp::Node & node) - : visualizer(std::make_unique("local_planner", name)) + : visualizer(std::make_unique("local_planner/" + name)) { world_model = std::make_shared(node); } virtual crane_msgs::msg::RobotCommands calculateRobotCommand( const crane_msgs::msg::RobotCommands & msg) = 0; - ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; WorldModelWrapper::SharedPtr world_model; }; diff --git a/crane_local_planner/include/crane_local_planner/simple_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_planner.hpp index 1381549f9..6dc376ce1 100644 --- a/crane_local_planner/include/crane_local_planner/simple_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/simple_planner.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -40,9 +40,18 @@ class SimplePlanner : public LocalPlannerBase for (auto & command : commands.robot_commands) { auto robot = world_model->getOurRobot(command.robot_id); if (not command.position_target_mode.empty()) { - visualizer->addLine( - robot->pose.pos.x(), robot->pose.pos.y(), command.position_target_mode.front().target_x, - command.position_target_mode.front().target_y, 1); + // visualizer->addLine( + // robot->pose.pos.x(), robot->pose.pos.y(), + // command.position_target_mode.front().target_x, + // command.position_target_mode.front().target_y, 1); + SvgLineBuilder line_builder; + line_builder.start(robot->pose.pos) + .end( + command.position_target_mode.front().target_x, + command.position_target_mode.front().target_y) + .stroke("red") + .strokeWidth(2); + visualizer->add(line_builder.getSvgString()); } if (command.local_planner_config.max_velocity > MAX_VEL) { command.local_planner_config.max_velocity = MAX_VEL; diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index 6c562f667..da18d21d6 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -56,10 +56,18 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo << "\" skill , but no position_target_mode is set."; RCLCPP_ERROR(get_logger(), what.str().c_str()); } else { - planner->visualizer->addLine( - raw_command.current_pose.x, raw_command.current_pose.y, - raw_command.position_target_mode.front().target_x, - raw_command.position_target_mode.front().target_y, 1, "yellow", 0.3); + // planner->visualizer->addLine( + // raw_command.current_pose.x, raw_command.current_pose.y, + // raw_command.position_target_mode.front().target_x, + // raw_command.position_target_mode.front().target_y, 1, "yellow", 0.3); + SvgLineBuilder line_builder; + line_builder.start(raw_command.current_pose.x, raw_command.current_pose.y) + .end( + raw_command.position_target_mode.front().target_x, + raw_command.position_target_mode.front().target_y) + .stroke("yellow", 0.3) + .strokeWidth(2); + planner->visualizer->add(line_builder.getSvgString()); } break; case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: @@ -114,33 +122,62 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo auto robot = world_model->getOurRobot(command.robot_id); switch (command.control_mode) { case crane_msgs::msg::RobotCommand::POSITION_TARGET_MODE: { - planner->visualizer->addLine( - robot->pose.pos.x(), robot->pose.pos.y(), command.position_target_mode.front().target_x, - command.position_target_mode.front().target_y, 1, "yellow", 0.5); + // planner->visualizer->addLine( + // robot->pose.pos.x(), robot->pose.pos.y(), + // command.position_target_mode.front().target_x, + // command.position_target_mode.front().target_y, 1, "yellow", 0.5); + SvgLineBuilder line_builder; + line_builder.start(robot->pose.pos) + .end( + command.position_target_mode.front().target_x, + command.position_target_mode.front().target_y) + .stroke("yellow", 0.5) + .strokeWidth(1); + planner->visualizer->add(line_builder.getSvgString()); } break; case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: { - planner->visualizer->addLine( - robot->pose.pos.x(), robot->pose.pos.y(), - robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx * 0.5, - robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy * 0.5, 1, - "white", 0.5); + // planner->visualizer->addLine( + // robot->pose.pos.x(), robot->pose.pos.y(), + // robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx * 0.5, + // robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy * 0.5, 1, + // "white", 0.5); + SvgLineBuilder line_builder; + line_builder.start(robot->pose.pos) + .end( + robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx * 0.5, + robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy * 0.5) + .stroke("white", 0.5) + .strokeWidth(1); + planner->visualizer->add(line_builder.getSvgString()); } break; case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: { - planner->visualizer->addLine( - robot->pose.pos.x(), robot->pose.pos.y(), - robot->pose.pos.x() + - 0.5 * command.polar_velocity_target_mode.front().target_velocity_r * - std::cos(command.polar_velocity_target_mode.front().target_velocity_theta), - robot->pose.pos.y() + - 0.5 * command.polar_velocity_target_mode.front().target_velocity_r * - std::sin(command.polar_velocity_target_mode.front().target_velocity_theta), - 1, "white", 0.5); + // planner->visualizer->addLine( + // robot->pose.pos.x(), robot->pose.pos.y(), + // robot->pose.pos.x() + + // 0.5 * command.polar_velocity_target_mode.front().target_velocity_r * + // std::cos(command.polar_velocity_target_mode.front().target_velocity_theta), + // robot->pose.pos.y() + + // 0.5 * command.polar_velocity_target_mode.front().target_velocity_r * + // std::sin(command.polar_velocity_target_mode.front().target_velocity_theta), + // 1, "white", 0.5); + SvgLineBuilder line_builder; + line_builder.start(robot->pose.pos) + .end( + robot->pose.pos.x() + + 0.5 * command.polar_velocity_target_mode.front().target_velocity_r * + std::cos(command.polar_velocity_target_mode.front().target_velocity_theta), + robot->pose.pos.y() + + 0.5 * command.polar_velocity_target_mode.front().target_velocity_r * + std::sin(command.polar_velocity_target_mode.front().target_velocity_theta)) + .stroke("white", 0.5) + .strokeWidth(1); + planner->visualizer->add(line_builder.getSvgString()); } break; } } planner->visualizer->flush(); - crane::ConsaiVisualizerBuffer::publish(); + crane::CraneVisualizerBuffer::publish(); } } // namespace crane diff --git a/crane_msgs/CMakeLists.txt b/crane_msgs/CMakeLists.txt index 82248a2b3..b074ea83f 100755 --- a/crane_msgs/CMakeLists.txt +++ b/crane_msgs/CMakeLists.txt @@ -66,7 +66,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} ${action_files} - DEPENDENCIES builtin_interfaces std_msgs geometry_msgs + DEPENDENCIES builtin_interfaces std_msgs geometry_msgs robocup_ssl_msgs ) if(BUILD_TESTING) diff --git a/crane_msgs/msg/analysis/PlaySituation.msg b/crane_msgs/msg/analysis/PlaySituation.msg index 02f767930..478a87d66 100755 --- a/crane_msgs/msg/analysis/PlaySituation.msg +++ b/crane_msgs/msg/analysis/PlaySituation.msg @@ -89,3 +89,5 @@ string referee_text string reason_text geometry_msgs/Point placement_position + +robocup_ssl_msgs/Referee referee_raw diff --git a/crane_msgs/package.xml b/crane_msgs/package.xml index 11be1569e..5569f6719 100755 --- a/crane_msgs/package.xml +++ b/crane_msgs/package.xml @@ -14,6 +14,7 @@ builtin_interfaces geometry_msgs + robocup_ssl_msgs std_msgs ament_lint_auto diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp index d5dc6e9e0..c0dbfd170 100644 --- a/crane_play_switcher/src/play_switcher.cpp +++ b/crane_play_switcher/src/play_switcher.cpp @@ -82,6 +82,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) // TODO(HansRobo): robocup_ssl_msgs/msg/Refereeをもう少しわかりやすい形式にする必要あり play_situation_msg.stage = msg.stage; play_situation_msg.command_raw = msg.command; + play_situation_msg.referee_raw = msg; if (latest_raw_referee_command != static_cast(msg.command)) { //-----------------------------------// diff --git a/crane_robot_receiver/src/ping_status_node.cpp b/crane_robot_receiver/src/ping_status_node.cpp index 88e15e2b7..c747718f8 100644 --- a/crane_robot_receiver/src/ping_status_node.cpp +++ b/crane_robot_receiver/src/ping_status_node.cpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/crane_robot_receiver/src/robot_receiver_node.cpp b/crane_robot_receiver/src/robot_receiver_node.cpp index 7dccdb70c..32f8cec3c 100644 --- a/crane_robot_receiver/src/robot_receiver_node.cpp +++ b/crane_robot_receiver/src/robot_receiver_node.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -332,7 +332,7 @@ class RobotReceiverNode : public rclcpp::Node explicit RobotReceiverNode(uint8_t robot_num = 10) : rclcpp::Node("robot_receiver_node"), clock(RCL_ROS_TIME) { - crane::ConsaiVisualizerBuffer::activate(*this); + crane::CraneVisualizerBuffer::activate(*this); publisher = create_publisher("/robot_feedback", 10); for (int i = 0; i < robot_num; i++) { @@ -405,7 +405,7 @@ class RobotReceiverNode : public rclcpp::Node } publisher->publish(msg); visualizer->flush(); - crane::ConsaiVisualizerBuffer::publish(); + crane::CraneVisualizerBuffer::publish(); }); } @@ -415,8 +415,8 @@ class RobotReceiverNode : public rclcpp::Node rclcpp::Publisher::SharedPtr publisher; - crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer = - std::make_unique("robot_receiver"); + crane::CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer = + std::make_unique("robot_receiver"); rclcpp::Clock clock; }; diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 3193bbe3a..fffe99c5f 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -8,7 +8,7 @@ #define CRANE_ROBOT_SKILLS__SKILL_BASE_HPP_ #include <../magic_enum.hpp> -#include +#include #include #include #include @@ -127,7 +127,7 @@ class SkillInterface const std::string & name, uint8_t id, const std::shared_ptr & wm) : name(name), command_base(std::make_shared(name, id, wm)), - visualizer(std::make_unique("skill", name)), + visualizer(std::make_unique("skill/" + name)), target_theta_context(getContextReference("target_theta")), dribble_power_context(getContextReference("dribble_power")), kick_power_context(getContextReference("kick_power")), @@ -139,7 +139,7 @@ class SkillInterface SkillInterface(const std::string & name, RobotCommandWrapperBase::SharedPtr command) : name(name), command_base(command), - visualizer(std::make_unique("skill", name)), + visualizer(std::make_unique("skill/" + name)), target_theta_context(getContextReference("target_theta")), dribble_power_context(getContextReference("dribble_power")), kick_power_context(getContextReference("kick_power")), @@ -235,7 +235,7 @@ class SkillInterface std::unordered_map contexts; - crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + crane::CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; Status status = Status::RUNNING; diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 7313597a7..04e419e0c 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -110,7 +110,15 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) }); addStateFunction(AttackerState::STEAL_BALL, [this]() -> Status { - visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0); + // visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0); + SvgCircleBuilder circle_builder; + circle_builder.center(world_model()->ball.pos) + .radius(0.25) + .stroke("blue") + .fill("white") + .strokeWidth(1); + visualizer->add(circle_builder.getSvgString()); + return steal_ball_skill.run(); }); @@ -270,7 +278,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - visualizer->addLine(world_model()->ball.pos, kick_target, 1, "red"); + // visualizer->addLine(world_model()->ball.pos, kick_target, 1, "red"); + SvgLineBuilder line_builder; + line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(1); + visualizer->add(line_builder.getSvgString()); kick_skill.setParameter("target", kick_target); Segment ball_to_target{world_model()->ball.pos, kick_target}; diff --git a/crane_robot_skills/src/goal_kick.cpp b/crane_robot_skills/src/goal_kick.cpp index 0b50328e3..3c8152d5d 100644 --- a/crane_robot_skills/src/goal_kick.cpp +++ b/crane_robot_skills/src/goal_kick.cpp @@ -27,7 +27,10 @@ Status GoalKick::update() world_model()); Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; - visualizer->addLine(world_model()->ball.pos, target, 2, "red"); + // visualizer->addLine(world_model()->ball.pos, target, 2, "red"); + SvgLineBuilder line_builder; + line_builder.start(world_model()->ball.pos).end(target).stroke("red").strokeWidth(2); + visualizer->add(line_builder.getSvgString()); kick_skill.setParameter("target", target); kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); return kick_skill.run(); diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index bdb81e54c..8caf1204f 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -49,7 +49,13 @@ Status Goalie::update() } } - visualizer->addPoint(robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "white", 1., phase); + // visualizer->addPoint(robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "white", 1., phase); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(phase) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); return Status::RUNNING; } @@ -81,7 +87,10 @@ void Goalie::emitBallFromPenaltyArea() } }(); - visualizer->addLine(ball, pass_target, 1, "blue"); + // visualizer->addLine(ball, pass_target, 1, "blue"); + SvgLineBuilder line_builder; + line_builder.start(ball).end(pass_target).stroke("blue").strokeWidth(1); + visualizer->add(line_builder.getSvgString()); Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; kick_skill.setParameter("target", pass_target); diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index f3d92f1b6..17d63120e 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -35,14 +35,26 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) receive_skill->setParameter("redirect_kick_power", 0.3); addStateFunction(KickState::ENTRY_POINT, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::ENTRY_POINT"); + // visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::ENTRY_POINT"); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("Kick::ENTRY_POINT") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); return Status::RUNNING; }); addTransition(KickState::ENTRY_POINT, KickState::AROUND_BALL_AND_KICK, [this]() { return true; }); addStateFunction(KickState::POSITIVE_REDIRECT_KICK, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK"); + // visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK"); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("Kick::POSITIVE_REDIRECT_KICK") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); // ボールラインに沿って追いかけつつ、角度はtargetへ向ける const auto & ball_pos = world_model()->ball.pos; command.lookAtFrom(getParameter("target"), ball_pos); @@ -77,7 +89,13 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) }); addStateFunction(KickState::REDIRECT_KICK, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK"); + // visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK"); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("Kick::REDIRECT_KICK") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); receive_skill->setParameter("target", getParameter("target")); if (robot()->getDistance(world_model()->ball.pos) < 0.5) { receive_skill->setParameter("policy", std::string("closest")); @@ -102,17 +120,44 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) addStateFunction(KickState::AROUND_BALL_AND_KICK, [this]() { auto target = getParameter("target"); Point ball_pos = world_model()->ball.pos; - visualizer->addLine(ball_pos, ball_pos + (target - ball_pos).normalized() * 1.0, 1, "blue"); + // visualizer->addLine(ball_pos, ball_pos + (target - ball_pos).normalized() * 1.0, 1, "blue"); + SvgLineBuilder line_builder; + line_builder.start(ball_pos) + .end(ball_pos + (target - ball_pos).normalized() * 1.0) + .stroke("blue") + .strokeWidth(1); + visualizer->add(line_builder.getSvgString()); constexpr double SWITCH_DISTANCE = 1.0; - visualizer->addCircle(ball_pos, SWITCH_DISTANCE, 1, "yellow", "yellow", 0.); + // visualizer->addCircle(ball_pos, SWITCH_DISTANCE, 1, "yellow", "yellow", 0.); + { + SvgCircleBuilder circle_builder; + circle_builder.center(ball_pos).radius(SWITCH_DISTANCE).stroke("yellow").strokeWidth(1); + visualizer->add(circle_builder.getSvgString()); + } if (robot()->getDistance(ball_pos) > SWITCH_DISTANCE) { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)"); + // visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)"); + { + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("Kick::AROUND_BALL(遠い)") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); + } command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3) .lookAtFrom(target, ball_pos) .setTerminalVelocity(0.3); return Status::RUNNING; } else { - visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）"); + // visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）"); + { + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("Kick::AROUND_BALL（近い）") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); + } auto calculateRatio = [](const double distance, const double min_distance, const double max_distance) { return (distance - min_distance) / (max_distance - min_distance); diff --git a/crane_robot_skills/src/marker.cpp b/crane_robot_skills/src/marker.cpp index 94f5b1ccd..8d55e1ab7 100644 --- a/crane_robot_skills/src/marker.cpp +++ b/crane_robot_skills/src/marker.cpp @@ -38,9 +38,18 @@ Status Marker::update() } command.setTargetPosition(marking_point, 0.1).setTargetTheta(target_theta); - visualizer->addCircle(enemy_pos, 0.3, 1, "black", ""); - visualizer->addLine( - robot()->pose.pos, enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3, 2, "black"); + // visualizer->addCircle(enemy_pos, 0.3, 1, "black", ""); + SvgCircleBuilder circle_builder; + circle_builder.center(enemy_pos).radius(0.3).stroke("black").strokeWidth(1); + visualizer->add(circle_builder.getSvgString()); + // visualizer->addLine( + // robot()->pose.pos, enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3, 2, "black"); + SvgLineBuilder line_builder; + line_builder.start(robot()->pose.pos) + .end(enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3) + .stroke("black") + .strokeWidth(2); + visualizer->add(line_builder.getSvgString()); return Status::RUNNING; } } // namespace crane::skills diff --git a/crane_robot_skills/src/penalty_kick.cpp b/crane_robot_skills/src/penalty_kick.cpp index 8e252e695..78d28ed4c 100644 --- a/crane_robot_skills/src/penalty_kick.cpp +++ b/crane_robot_skills/src/penalty_kick.cpp @@ -46,7 +46,9 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base) double best_angle = GoalKick::getBestAngleToShootFromPoint( minimum_angle_accuracy, world_model()->ball.pos, world_model()); Point best_target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; - visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target"); + // visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target"); + SvgCircleBuilder circle_builder; + circle_builder.center(best_target).radius(0.1).stroke("red").strokeWidth(1); kick_skill.setParameter("target", best_target); diff --git a/crane_robot_skills/src/receive.cpp b/crane_robot_skills/src/receive.cpp index a704ceaba..a41455831 100644 --- a/crane_robot_skills/src/receive.cpp +++ b/crane_robot_skills/src/receive.cpp @@ -48,7 +48,10 @@ Status Receive::update() }(); Point interception_point = getInterceptionPoint() + offset; - visualizer->addLine(interception_point, robot()->pose.pos, 1, "red", 1., "intercept"); + // visualizer->addLine(interception_point, robot()->pose.pos, 1, "red", 1., "intercept"); + SvgLineBuilder line_builder; + line_builder.start(interception_point).end(robot()->pose.pos).stroke("red").strokeWidth(1); + visualizer->add(line_builder.getSvgString()); if (getParameter("enable_redirect")) { Point redirect_target = getParameter("redirect_target"); @@ -85,7 +88,10 @@ Point Receive::getInterceptionPoint() const Segment ball_line( world_model()->ball.pos, (world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0)); - visualizer->addLine(ball_line.first, ball_line.second, 1, "blue", 1., "ball_line"); + // visualizer->addLine(ball_line.first, ball_line.second, 1, "blue", 1., "ball_line"); + SvgLineBuilder line_builder; + line_builder.start(ball_line.first).end(ball_line.second).stroke("blue").strokeWidth(1); + visualizer->add(line_builder.getSvgString()); auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line); return result.closest_point; } else { diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index a8197a59b..560ac3d17 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -20,7 +20,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba setParameter("コート端判定のオフセット", 0.0); addStateFunction(SingleBallPlacementStates::ENTRY_POINT, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); command.stopHere(); command.setOmegaLimit(10.0); return Status::RUNNING; @@ -41,7 +47,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // 端にある場合、コート側からアプローチする addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); if (not pull_back_target) { pull_back_target = world_model()->ball.pos; const auto offset = getParameter("コート端判定のオフセット"); @@ -95,7 +107,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // PULL_BACK_FROM_EDGE_TOUCH addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); command.disablePlacementAvoidance() .disableBallAvoidance() .disableGoalAreaAvoidance() @@ -151,7 +169,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // PULL_BACK_FROM_EDGE_PULL addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); command.setDribblerTargetPosition(pull_back_target.value()); // 角度はそのまま引っ張りたいので指定はしない command.dribble(0.6); @@ -176,7 +200,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba [this]() { return robot()->getDistance(world_model()->ball.pos) > 0.15; }); addStateFunction(SingleBallPlacementStates::GO_OVER_BALL, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); command.setMaxVelocity(1.5); Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); @@ -225,7 +255,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba [this]() { return skill_status == Status::SUCCESS; }); addStateFunction(SingleBallPlacementStates::CONTACT_BALL, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); if (not get_ball_contact) { get_ball_contact = std::make_shared(command_base); } @@ -243,7 +279,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba [this]() { return skill_status == Status::SUCCESS; }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); if (not move_with_ball) { move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); @@ -277,7 +319,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba [this]() { return skill_status == Status::FAILURE; }); addStateFunction(SingleBallPlacementStates::SLEEP, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); if (not sleep) { sleep = std::make_shared(command_base); sleep->setParameter("duration", 2.0); @@ -298,7 +346,13 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addStateFunction(SingleBallPlacementStates::LEAVE_BALL, [this]() { - visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + // visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text(state_string) + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); if (not set_target_position) { set_target_position = std::make_shared(command_base); } diff --git a/crane_robot_skills/src/sub_attacker.cpp b/crane_robot_skills/src/sub_attacker.cpp index e6f65dd3c..e3523a116 100644 --- a/crane_robot_skills/src/sub_attacker.cpp +++ b/crane_robot_skills/src/sub_attacker.cpp @@ -48,12 +48,29 @@ Status SubAttacker::update() command.setTargetPosition( result.closest_point + (robot()->pose.pos - result.closest_point).normalized() * 0.5); command.enableBallAvoidance(); - visualizer->addPoint( - robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ボールラインから一旦遠ざかる"); + { + // visualizer->addPoint( + // robot()->pose.pos.x(), robot()->pose.pos.y(), 0, + // "red", 1., "ボールラインから一旦遠ざかる"); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("ボールラインから一旦遠ざかる") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); + } } else { // ボールの進路上に移動 - visualizer->addPoint( - robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ボールの進路上に移動"); + { + // visualizer->addPoint( + // robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ボールの進路上に移動"); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("ボールの進路上に移動") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); + } auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line); // ゴールとボールの中間方向を向く @@ -71,8 +88,16 @@ Status SubAttacker::update() command.setTargetPosition(result.closest_point - (2 * to_goal + to_ball).normalized() * 0.13); } } else { - visualizer->addPoint( - robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ベストポジションへ移動"); + { + // visualizer->addPoint( + // robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ベストポジションへ移動"); + SvgTextBuilder text_builder; + text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("ベストポジションへ移動") + .fill("white") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); + } Point best_position = robot()->pose.pos; double best_score = 0.0; for (const auto & dpps_point : dpps_points) { @@ -96,8 +121,13 @@ Status SubAttacker::update() auto [goal_angle, width] = world_model()->getLargestGoalAngleRangeFromPoint(target_pos); auto to_goal = getNormVec(goal_angle); auto to_ball = (world_model()->ball.pos - target_pos).normalized(); - visualizer->addLine( - target_pos, target_pos + to_goal * 3.0, 2, "yellow", 1.0, "Supporterシュートライン"); + { + // visualizer->addLine( + // target_pos, target_pos + to_goal * 3.0, 2, "yellow", 1.0, "Supporterシュートライン"); + SvgLineBuilder line_builder; + line_builder.start(target_pos).end(target_pos + to_goal * 3.0).stroke("yellow").strokeWidth(2); + visualizer->add(line_builder.getSvgString()); + } command.setTargetTheta(getAngle(to_goal + to_ball)); command.liftUpDribbler(); command.kickStraight(getParameter("kicker_power")); diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp index 3557add9c..1bd1c1664 100644 --- a/crane_simple_ai/include/crane_commander.hpp +++ b/crane_simple_ai/include/crane_commander.hpp @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include #include @@ -95,7 +95,7 @@ class ROSNode : public rclcpp::Node public: ROSNode() : Node("crane_commander") { - crane::ConsaiVisualizerBuffer::activate(*this); + crane::CraneVisualizerBuffer::activate(*this); world_model = std::make_shared(*this); subscription_robot_feedback = create_subscription( @@ -109,8 +109,8 @@ class ROSNode : public rclcpp::Node crane_msgs::msg::RobotFeedbackArray robot_feedback_array; - crane::ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer = - std::make_unique("simple_ai"); + crane::CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer = + std::make_unique("simple_ai"); }; class CraneCommander : public QMainWindow diff --git a/crane_visualization_interfaces/proto/ObjectsArray.proto b/crane_visualization_interfaces/proto/ObjectsArray.proto index a4a9c1f47..36a0d1826 100644 --- a/crane_visualization_interfaces/proto/ObjectsArray.proto +++ b/crane_visualization_interfaces/proto/ObjectsArray.proto @@ -4,141 +4,11 @@ syntax = "proto3"; package visualizer; -// Color.msg -message Color { - string name = 1; - float red = 2; - float green = 3; - float blue = 4; - float alpha = 5; -} - -// Point.msg -message Point { - float x = 1; // unit: meters - float y = 2; // unit: meters -} - -// ShapeAnnotation.msg -message ShapeAnnotation { - float normalized_x = 1; - float normalized_y = 2; - float normalized_width = 3; - float normalized_height = 4; - string text = 5; - Color color = 6; -} - -// ShapeArc.msg -message ShapeArc { - Point center = 1; - float radius = 2; - float start_angle = 3; - float end_angle = 4; - uint32 size = 5; - Color color = 6; - string caption = 7; -} - -// ShapeCircle.msg -message ShapeCircle { - Point center = 1; - float radius = 2; - Color line_color = 3; - Color fill_color = 4; - uint32 line_size = 5; - string caption = 6; -} - -// ShapeLine.msg -message ShapeLine { - Point p1 = 1; - Point p2 = 2; - uint32 size = 3; - Color color = 4; - string caption = 5; -} - -// ShapePoint.msg -message ShapePoint { - float x = 1; // unit: meters - float y = 2; // unit: meters - uint32 size = 3; - Color color = 4; - string caption = 5; -} - -// ShapeRectangle.msg -message ShapeRectangle { - Point center = 1; - float width = 2; - float height = 3; - uint32 line_size = 4; - Color line_color = 5; - Color fill_color = 6; - string caption = 7; -} - -// ShapeRobot.msg -message ShapeRobot { - enum ColorType { - COLOR_TYPE_DEFAULT = 0; - COLOR_TYPE_REAL = 1; - } - - ColorType color_type = 1; - float radius = 2; - Color line_color = 3; - Color fill_color = 4; - uint32 line_size = 5; - uint32 id = 6; - float x = 7; - float y = 8; - float theta = 9; - string caption = 10; -} - -// ShapeText.msg -message ShapeText { - float x = 1; // unit: meters - float y = 2; // unit: meters - string text = 3; - uint32 size = 4; - Color color = 5; -} - -// ShapeTube.msg -message ShapeTube { - Point p1 = 1; - Point p2 = 2; - float radius = 3; - Color line_color = 4; - Color fill_color = 5; - uint32 line_size = 6; - string caption = 7; -} - -// Objects.msg -message Objects { +message SvgPrimitiveArray { string layer = 1; - string sub_layer = 2; - uint32 z_order = 3; // Higher numbers are displayed in front - - // Objects drawn in the window area - repeated ShapeAnnotation annotations = 4; - - // Objects drawn in the field area - repeated ShapePoint points = 5; - repeated ShapeLine lines = 6; - repeated ShapeArc arcs = 7; - repeated ShapeRectangle rects = 8; - repeated ShapeCircle circles = 9; - repeated ShapeTube tubes = 10; - repeated ShapeText texts = 11; - repeated ShapeRobot robots = 12; + repeated string svg_primitives = 2; } -// ObjectsArray.msg -message ObjectsArray { - repeated Objects objects = 1; +message SvgLayerArray { + repeated SvgPrimitiveArray svg_primitive_arrays = 1; } diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp index 2f70440f9..be90c626c 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp @@ -18,15 +18,12 @@ #include #include -#include +#include #include #include namespace crane { - -using VisualizerObjects = crane_visualization_interfaces::msg::Objects; -using VisualizerObjectsArray = crane_visualization_interfaces::msg::ObjectsArray; using Referee = robocup_ssl_msgs::msg::Referee; class VisualizationDataHandler @@ -42,7 +39,9 @@ class VisualizationDataHandler private: rclcpp::Subscription::SharedPtr sub_referee_; - rclcpp::Publisher::SharedPtr pub_vis_objects_; + std::shared_ptr visualizer_geometry; + std::shared_ptr visualizer_tracked; + std::shared_ptr visualizer_referee; double ball_x; 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 75242ef47..c6c9ed517 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 @@ -52,7 +52,7 @@ extern "C" { #include #include -#include +#include #include #include #include @@ -178,7 +178,7 @@ class WorldModelPublisherComponent : public rclcpp::Node bool ball_event_detected = false; - ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; std::array, 20> friend_history; std::array, 20> enemy_history; diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp index 58723eda4..9d70707ae 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -14,33 +14,84 @@ #include "crane_world_model_publisher/visualization_data_handler.hpp" -#include -#include -#include -#include -#include -#include -#include +#include #include namespace crane { -using VisColor = crane_visualization_interfaces::msg::Color; -using VisArc = crane_visualization_interfaces::msg::ShapeArc; -using VisAnnotation = crane_visualization_interfaces::msg::ShapeAnnotation; -using VisCircle = crane_visualization_interfaces::msg::ShapeCircle; -using VisLine = crane_visualization_interfaces::msg::ShapeLine; -using VisPoint = crane_visualization_interfaces::msg::ShapePoint; -using VisRect = crane_visualization_interfaces::msg::ShapeRectangle; -using VisRobot = crane_visualization_interfaces::msg::ShapeRobot; -using VisText = crane_visualization_interfaces::msg::ShapeText; -using VisTube = crane_visualization_interfaces::msg::ShapeTube; using RobotId = robocup_ssl_msgs::msg::RobotId; +struct SvgRobotBuilder : public SvgPathBuilder +{ + SvgRobotBuilder() : corner_angle(std::acos(center_to_dribbler / radius)) {} + + std::string getSvgString() const override + { + SvgPathDefinitionBuilder path_builder; + path_builder + .moveTo(robot_position.x() + botRightX(theta), robot_position.y() + botRightY(theta)) + .arcTo( + {radius, radius}, 0, true, true, + {robot_position.x() + botLeftX(theta), robot_position.y() + botLeftY(theta)}) + .lineTo(robot_position.x() + botRightX(theta), robot_position.y() + botRightY(theta)); + + SvgPathBuilder builder = path_builder.build(); + builder.fill(fill_color, fill_opacity) + .stroke(stroke_color, stroke_opacity) + .strokeWidth(stroke_width); + + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgRobotBuilder & position(Point p, double theta) + { + this->robot_position = p; + this->theta = theta; + return *this; + } + + SvgRobotBuilder & position(double x, double y, double theta) + { + return position(Point(x, y), theta); + } + +private: + Point robot_position; + double theta; + + double botRightX(double orientation) const + { + return radius * std::cos(orientation + corner_angle); + } + double botRightY(double orientation) const + { + return radius * std::sin(orientation + corner_angle); + } + double botLeftX(double orientation) const + { + return radius * std::cos(orientation - corner_angle); + } + double botLeftY(double orientation) const + { + return radius * std::sin(orientation - corner_angle); + } + const double radius = 0.085; + const double center_to_dribbler = 0.055; + const double corner_angle; +}; + VisualizationDataHandler::VisualizationDataHandler(rclcpp::Node & node) +: visualizer_geometry( + std::make_shared("world_model/geometry")), + visualizer_tracked( + std::make_shared("world_model/tracked")), + visualizer_referee(std::make_shared("world_model/referee")) { - pub_vis_objects_ = - node.create_publisher("visualizer_objects", rclcpp::SensorDataQoS()); + CraneVisualizerBuffer::activate(node); sub_referee_ = node.create_subscription( "referee", 10, std::bind(&VisualizationDataHandler::publish_vis_referee, this, std::placeholders::_1)); @@ -49,112 +100,80 @@ VisualizationDataHandler::VisualizationDataHandler(rclcpp::Node & node) void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geometry_data) { // geometryを描画情報に変換してpublishする - auto vis_objects_array = std::make_unique(); - - VisualizerObjects vis_objects; - vis_objects.layer = "vision"; - vis_objects.sub_layer = "geometry"; - vis_objects.z_order = 0; for (const auto & field_line : geometry_data.field().field_lines()) { - VisLine line; - - line.color.name = "white"; - line.color.alpha = 1.0; - line.size = 2; - // 単位を[m]に変換 - line.p1.x = field_line.p1().x() * 0.001; - line.p1.y = field_line.p1().y() * 0.001; - line.p2.x = field_line.p2().x() * 0.001; - line.p2.y = field_line.p2().y() * 0.001; - // line.caption = field_line.name; - - vis_objects.lines.push_back(line); + SvgLineBuilder builder; + builder.start(field_line.p1().x() * 0.001, field_line.p1().y() * 0.001) + .end(field_line.p2().x() * 0.001, field_line.p2().y() * 0.001) + .stroke("white") + .strokeWidth(2); + visualizer_geometry->add(builder.getSvgString()); } for (const auto & field_arc : geometry_data.field().field_arcs()) { - VisArc arc; - - arc.color.name = "white"; - arc.color.alpha = 1.0; - arc.size = 2; - // 単位を[m]に変換 - arc.center.x = field_arc.center().x() * 0.001; - arc.center.y = field_arc.center().y() * 0.001; - arc.radius = field_arc.radius() * 0.001; - arc.start_angle = field_arc.a1(); - arc.end_angle = field_arc.a2(); - // arc.caption = field_arc.name; - - vis_objects.arcs.push_back(arc); + SvgCircleBuilder builder; + builder.center(field_arc.center().x() * 0.001, field_arc.center().y() * 0.001) + .radius(field_arc.radius() * 0.001) + .stroke("white") + .strokeWidth(2); + visualizer_geometry->add(builder.getSvgString()); } // ペナルティマーク // Ref: https://robocup-ssl.github.io/ssl-rules/sslrules.html#_penalty_mark - VisPoint point; - point.color.name = "white"; - point.color.alpha = 1.0; - point.size = 6; - point.x = -geometry_data.field().field_length() * 0.001 / 2.0 + 8.0; - point.y = 0.0; - // point.caption = "penalty_mark_positive"; - vis_objects.points.push_back(point); - - point.x = -point.x; - // point.caption = "penalty_mark_negative"; - vis_objects.points.push_back(point); + SvgCircleBuilder builder; + builder.center(-geometry_data.field().field_length() * 0.001 / 2.0 + 8.0, 0.0) + .radius(0.006) + .fill("white"); + visualizer_geometry->add(builder.getSvgString()); + + builder.center(geometry_data.field().field_length() * 0.001 / 2.0 - 8.0, 0.0) + .radius(0.006) + .fill("white"); + visualizer_geometry->add(builder.getSvgString()); // フィールドの枠 - VisRect rect; - rect.line_color.name = "black"; - rect.line_color.alpha = 1.0; - rect.fill_color.alpha = 0.0; - rect.line_size = 3; - rect.center.x = 0.0; - rect.center.y = 0.0; - rect.width = - (geometry_data.field().field_length() + geometry_data.field().boundary_width() * 2) * 0.001; - rect.height = - (geometry_data.field().field_width() + geometry_data.field().boundary_width() * 2) * 0.001; - // rect.caption = "wall"; - vis_objects.rects.push_back(rect); - - vis_objects_array->objects.push_back(vis_objects); - pub_vis_objects_->publish(std::move(vis_objects_array)); + SvgRectBuilder rect_builder; + rect_builder + .top_left( + -(geometry_data.field().field_length() + geometry_data.field().boundary_width() * 2) * 0.001 / + 2.0, + -(geometry_data.field().field_width() + geometry_data.field().boundary_width() * 2) * 0.001 / + 2.0) + .size( + (geometry_data.field().field_length() + geometry_data.field().boundary_width() * 2) * 0.001, + (geometry_data.field().field_width() + geometry_data.field().boundary_width() * 2) * 0.001) + .stroke("black") + .strokeWidth(3); + visualizer_geometry->add(rect_builder.getSvgString()); + visualizer_geometry->flush(); + CraneVisualizerBuffer::publish(); } void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_frame) { const double VELOCITY_ALPHA = 0.5; // tracked_frameを描画情報に変換してpublishする - auto vis_objects_array = std::make_unique(); - - VisualizerObjects vis_objects; - vis_objects.layer = "vision"; - vis_objects.sub_layer = "tracked"; - vis_objects.z_order = 10; // 一番上に描画する - - VisCircle vis_ball; - vis_ball.line_color.name = "black"; - vis_ball.fill_color.name = "orange"; - vis_ball.line_size = 1; - vis_ball.radius = 0.0215; + for (const auto & ball : tracked_frame.balls()) { if (!ball.has_visibility() || ball.visibility() < 0.5) { continue; } - vis_ball.center.x = ball.pos().x(); - vis_ball.center.y = ball.pos().y(); - vis_objects.circles.push_back(vis_ball); + SvgCircleBuilder builder; + builder.center(ball.pos().x(), ball.pos().y()) + .radius(0.0215) + .stroke("black") + .fill("orange") + .strokeWidth(1); + visualizer_tracked->add(builder.getSvgString()); // ボールは小さいのでボールの周りを大きな円で囲う - vis_ball.line_color.name = "crimson"; - vis_ball.fill_color.alpha = 0.0; - vis_ball.line_color.alpha = 0.7; - vis_ball.line_size = 1; - vis_ball.radius = 0.5; - vis_ball.caption = "ball is here"; - vis_objects.circles.push_back(vis_ball); + builder.center(ball.pos().x(), ball.pos().y()) + .radius(0.5) + .stroke("crimson", 0.7) + .fill("none") + .strokeWidth(1); + visualizer_tracked->add(builder.getSvgString()); ball_x = ball.pos().x(); ball_y = ball.pos().y(); @@ -162,41 +181,40 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ // 速度を描画 if (ball.has_vel()) { const double vel_norm = std::hypot(ball.vel().x(), ball.vel().y()); - VisLine ball_vel; - ball_vel.color.name = "gold"; - ball_vel.color.alpha = VELOCITY_ALPHA; - ball_vel.size = 2; - ball_vel.p1.x = ball.pos().x(); - ball_vel.p1.y = ball.pos().y(); - ball_vel.p2.x = ball.pos().x() + ball.vel().x(); - ball_vel.p2.y = ball.pos().y() + ball.vel().y(); - // ball_vel.caption = std::to_string(vel_norm); - vis_objects.lines.push_back(ball_vel); + SvgLineBuilder line_builder; + line_builder.start(ball.pos().x(), ball.pos().y()) + .end(ball.pos().x() + ball.vel().x(), ball.pos().y() + ball.vel().y()) + .stroke("gold", VELOCITY_ALPHA) + .strokeWidth(2); + visualizer_tracked->add(line_builder.getSvgString()); } } - VisRobot vis_robot; - vis_robot.line_color.name = "black"; - vis_robot.line_size = 1; - vis_robot.fill_color.alpha = 1.0; - vis_robot.line_color.alpha = 1.0; - vis_robot.radius = 0.09; for (const auto & robot : tracked_frame.robots()) { if (not robot.has_visibility() || robot.visibility() < 0.5) { continue; } - - vis_robot.id = robot.robot_id().id(); + SvgRobotBuilder builder; + double robot_x = robot.pos().x(); + double robot_y = robot.pos().y(); + double robot_theta = robot.orientation(); + builder.position(robot.pos().x(), robot.pos().y(), robot.orientation()) + .stroke("black") + .strokeWidth(1); if (robot.robot_id().team() == RobotId::TEAM_COLOR_BLUE) { - vis_robot.fill_color.name = "dodgerblue"; + builder.fill("dodgerblue"); } else { - vis_robot.fill_color.name = "yellow"; + builder.fill("yellow"); } + visualizer_tracked->add(builder.getSvgString()); - vis_robot.x = robot.pos().x(); - vis_robot.y = robot.pos().y(); - vis_robot.theta = robot.orientation(); - vis_objects.robots.push_back(vis_robot); + SvgTextBuilder text_id_builder; + text_id_builder.position(robot_x, robot_y + 0.05) + .text(std::to_string(robot.robot_id().id())) + .fill("black") + .fontSize(100) + .textAnchor("middle"); + visualizer_tracked->add(text_id_builder.getSvgString()); // 速度を描画 // if (robot.has_vel() && robot.hans_vel_angular()) { @@ -225,9 +243,8 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_ // vis_objects.lines.push_back(robot_vel); // } } - - vis_objects_array->objects.push_back(vis_objects); - pub_vis_objects_->publish(std::move(vis_objects_array)); + visualizer_tracked->flush(); + CraneVisualizerBuffer::publish(); } auto parse_stage = [](const auto & ref_stage) -> std::string { @@ -351,49 +368,39 @@ auto parse_command = []( void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) { // レフェリー情報を描画オブジェクトに変換してpublishする - const double MARGIN_X = 0.02; - const double TEXT_HEIGHT = 0.05; - const double STAGE_COMMAND_WIDTH = 0.15; - const double STAGE_COMMAND_X = 0.0 + MARGIN_X; - const double TIMER_WIDTH = 0.15; + const double MARGIN_X = 2.; + const double TEXT_HEIGHT = 300; + const double STAGE_COMMAND_WIDTH = 15; + const double STAGE_COMMAND_X = -50 + MARGIN_X; + const double TIMER_WIDTH = 15; const double TIMER_X = STAGE_COMMAND_X + STAGE_COMMAND_WIDTH + MARGIN_X; - const double BOTS_WIDTH = 0.2; + const double BOTS_WIDTH = 20; const double BOTS_X = TIMER_X + TIMER_WIDTH + MARGIN_X; - const double CARDS_WIDTH = 0.1; + const double CARDS_WIDTH = 10; const double CARDS_X = BOTS_X + BOTS_WIDTH + MARGIN_X; - const double YELLOW_CARD_TIMES_WIDTH = 0.1; + const double YELLOW_CARD_TIMES_WIDTH = 10; const double YELLOW_CARD_TIMES_X = CARDS_X + CARDS_WIDTH + MARGIN_X; - const double TIMEOUT_WIDTH = 0.2; + const double TIMEOUT_WIDTH = 20; const double TIMEOUT_X = YELLOW_CARD_TIMES_X + YELLOW_CARD_TIMES_WIDTH + MARGIN_X; + const double FIRST_LINE_Y = -60; + const double SECOND_LINE_Y = -55; const std::string COLOR_TEXT_BLUE = "deepskyblue"; const std::string COLOR_TEXT_YELLOW = "yellow"; const std::string COLOR_TEXT_WARNING = "red"; - auto vis_objects_array = std::make_unique(); - - VisualizerObjects vis_objects; - vis_objects.layer = "referee"; - vis_objects.sub_layer = "info"; - vis_objects.z_order = 2; - // STAGEとCOMMANDを表示 - VisAnnotation vis_annotation; - vis_annotation.text = parse_stage(msg->stage); - vis_annotation.color.name = "white"; - vis_annotation.color.alpha = 1.0; - vis_annotation.normalized_x = STAGE_COMMAND_X; - vis_annotation.normalized_y = 0.0; - vis_annotation.normalized_width = STAGE_COMMAND_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); - - vis_annotation.text = parse_command(*msg); - vis_annotation.color.name = "white"; - vis_annotation.normalized_x = STAGE_COMMAND_X; - vis_annotation.normalized_y = TEXT_HEIGHT; - vis_annotation.normalized_width = STAGE_COMMAND_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + SvgTextBuilder text_builder; + text_builder.viewBoxPosition(STAGE_COMMAND_X, SECOND_LINE_Y) + .text(parse_stage(msg->stage)) + .fill("white") + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); + + text_builder.viewBoxPosition(STAGE_COMMAND_X, FIRST_LINE_Y) + .text(parse_command(*msg)) + .fill("white") + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); // 残り時間とACT_TIMEを表示 if (msg->stage_time_left.size() > 0) { @@ -409,13 +416,11 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) }; return "STAGE: " + parse_microseconds_to_text(ref_stage_time_left); }; - vis_annotation.text = parse_stage_time_left(msg->stage_time_left.front()); - vis_annotation.color.name = "white"; - vis_annotation.normalized_x = TIMER_X; - vis_annotation.normalized_y = 0.0; - vis_annotation.normalized_width = TIMER_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + text_builder.viewBoxPosition(TIMER_X, SECOND_LINE_Y) + .text(parse_stage_time_left(msg->stage_time_left.front())) + .fill("white") + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); } if (msg->current_action_time_remaining.size() > 0) { @@ -430,50 +435,42 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) } return "ACT: " + text; }; - vis_annotation.text = parse_action_time_remaining(msg->current_action_time_remaining.front()); - vis_annotation.color.name = "white"; - vis_annotation.normalized_x = TIMER_X; - vis_annotation.normalized_y = TEXT_HEIGHT; - vis_annotation.normalized_width = TIMER_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + text_builder.viewBoxPosition(TIMER_X, FIRST_LINE_Y) + .text(parse_action_time_remaining(msg->current_action_time_remaining.front())) + .fill("white") + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); } // ロボット数 - vis_annotation.color.name = COLOR_TEXT_BLUE; - vis_annotation.text = "BLUE BOTS: " + std::to_string(msg->blue.max_allowed_bots[0]); - vis_annotation.normalized_x = BOTS_X; - vis_annotation.normalized_y = 0.0; - vis_annotation.normalized_width = BOTS_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); - - vis_annotation.color.name = COLOR_TEXT_YELLOW; - vis_annotation.text = "YELLOW BOTS: " + std::to_string(msg->yellow.max_allowed_bots[0]); - vis_annotation.normalized_x = BOTS_X; - vis_annotation.normalized_y = TEXT_HEIGHT; - vis_annotation.normalized_width = BOTS_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + text_builder.viewBoxPosition(BOTS_X, SECOND_LINE_Y) + .text("BLUE BOTS: " + std::to_string(msg->blue.max_allowed_bots[0])) + .fill(COLOR_TEXT_BLUE) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); + + text_builder.viewBoxPosition(BOTS_X, FIRST_LINE_Y) + .text("YELLOW BOTS: " + std::to_string(msg->yellow.max_allowed_bots[0])) + .fill(COLOR_TEXT_YELLOW) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); // カード数 - vis_annotation.color.name = COLOR_TEXT_BLUE; - vis_annotation.text = - "R: " + std::to_string(msg->blue.red_cards) + ", Y: " + std::to_string(msg->blue.yellow_cards); - vis_annotation.normalized_x = CARDS_X; - vis_annotation.normalized_y = 0.0; - vis_annotation.normalized_width = CARDS_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); - - vis_annotation.color.name = COLOR_TEXT_YELLOW; - vis_annotation.text = "R: " + std::to_string(msg->yellow.red_cards) + - ", Y: " + std::to_string(msg->yellow.yellow_cards); - vis_annotation.normalized_x = CARDS_X; - vis_annotation.normalized_y = TEXT_HEIGHT; - vis_annotation.normalized_width = CARDS_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + text_builder.viewBoxPosition(CARDS_X, SECOND_LINE_Y) + .text( + "R: " + std::to_string(msg->blue.red_cards) + + ", Y: " + std::to_string(msg->blue.yellow_cards)) + .fill(COLOR_TEXT_BLUE) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); + + text_builder.viewBoxPosition(CARDS_X, FIRST_LINE_Y) + .text( + "R: " + std::to_string(msg->yellow.red_cards) + + ", Y: " + std::to_string(msg->yellow.yellow_cards)) + .fill(COLOR_TEXT_YELLOW) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); // イエローカードの時間 auto parse_yellow_card_times = [](const auto & yellow_card_times) { @@ -490,65 +487,50 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg) } return text; }; - vis_annotation.color.name = COLOR_TEXT_BLUE; - vis_annotation.text = parse_yellow_card_times(msg->blue.yellow_card_times); - vis_annotation.normalized_x = YELLOW_CARD_TIMES_X; - - vis_annotation.normalized_y = 0.0; - vis_annotation.normalized_width = YELLOW_CARD_TIMES_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); - - vis_annotation.color.name = COLOR_TEXT_YELLOW; - vis_annotation.text = parse_yellow_card_times(msg->yellow.yellow_card_times); - vis_annotation.normalized_x = YELLOW_CARD_TIMES_X; - vis_annotation.normalized_y = TEXT_HEIGHT; - vis_annotation.normalized_width = YELLOW_CARD_TIMES_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + text_builder.viewBoxPosition(YELLOW_CARD_TIMES_X, SECOND_LINE_Y) + .text(parse_yellow_card_times(msg->blue.yellow_card_times)) + .fill(COLOR_TEXT_BLUE) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); + + text_builder.viewBoxPosition(YELLOW_CARD_TIMES_X, FIRST_LINE_Y) + .text(parse_yellow_card_times(msg->yellow.yellow_card_times)) + .fill(COLOR_TEXT_YELLOW) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); // タイムアウト auto parse_timeouts = [](const auto & timeouts, const auto & timeout_time) { return "Timeouts: " + std::to_string(timeouts) + "\n" + std::to_string(timeout_time); }; - vis_annotation.color.name = COLOR_TEXT_BLUE; - vis_annotation.text = parse_timeouts(msg->blue.timeouts, msg->blue.timeout_time); - vis_annotation.normalized_x = TIMEOUT_X; - vis_annotation.normalized_y = 0.0; - vis_annotation.normalized_width = TIMEOUT_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); - - vis_annotation.color.name = COLOR_TEXT_YELLOW; - vis_annotation.text = parse_timeouts(msg->yellow.timeouts, msg->yellow.timeout_time); - vis_annotation.normalized_x = TIMEOUT_X; - vis_annotation.normalized_y = TEXT_HEIGHT; - vis_annotation.normalized_width = TIMEOUT_WIDTH; - vis_annotation.normalized_height = TEXT_HEIGHT; - vis_objects.annotations.push_back(vis_annotation); + text_builder.viewBoxPosition(TIMEOUT_X, SECOND_LINE_Y) + .text(parse_timeouts(msg->blue.timeouts, msg->blue.timeout_time)) + .fill(COLOR_TEXT_BLUE) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); + + text_builder.viewBoxPosition(TIMEOUT_X, FIRST_LINE_Y) + .text(parse_timeouts(msg->yellow.timeouts, msg->yellow.timeout_time)) + .fill(COLOR_TEXT_YELLOW) + .fontSize(TEXT_HEIGHT); + visualizer_referee->add(text_builder.getSvgString()); // プレイスメント位置 if ( msg->command == Referee::COMMAND_BALL_PLACEMENT_BLUE || msg->command == Referee::COMMAND_BALL_PLACEMENT_YELLOW) { if (not msg->designated_position.empty()) { - VisTube vis_tube; - vis_tube.p1.x = msg->designated_position.front().x / 1000.; - vis_tube.p1.y = msg->designated_position.front().y / 1000.; - vis_tube.p2.x = ball_x; - vis_tube.p2.y = ball_y; - vis_tube.radius = 0.5; - vis_tube.line_color.name = "aquamarine"; - vis_tube.line_color.alpha = 1.0; - // vis_tube.fill_color.name = "aquamarine"; - vis_tube.fill_color.alpha = 0.0; - vis_tube.line_size = 1; - vis_tube.caption = "placement"; - vis_objects.tubes.push_back(vis_tube); + SvgLineBuilder line_builder; + line_builder + .start( + msg->designated_position.front().x / 1000., msg->designated_position.front().y / 1000.) + .end(ball_x, ball_y) + .stroke("aquamarine") + .strokeWidth(1); + visualizer_referee->add(line_builder.getSvgString()); } } - - vis_objects_array->objects.push_back(vis_objects); - pub_vis_objects_->publish(std::move(vis_objects_array)); + visualizer_referee->flush(); + CraneVisualizerBuffer::publish(); } } // namespace crane diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 44ffc5c96..eb49d45c7 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -13,10 +13,7 @@ namespace crane { WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOptions & options) -: rclcpp::Node("world_model_publisher", options), - vis_data_handler(*this), - visualizer( - std::make_unique("world_model_publisher", "trajectory")) +: rclcpp::Node("world_model_publisher", options), vis_data_handler(*this) { using std::chrono_literals::operator""ms; declare_parameter("tracker_address", "224.5.23.2"); @@ -30,9 +27,11 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt get_parameter("vision_address").get_value(), get_parameter("vision_port").get_value()); - crane::ConsaiVisualizerBuffer::activate(*this); + CraneVisualizerBuffer::activate(*this); + visualizer = + std::make_unique("world_model/trajectory"); - declare_parameter("position_history_size", 100); + declare_parameter("position_history_size", 200); get_parameter("position_history_size", history_size); udp_timer = rclcpp::create_timer( @@ -448,7 +447,13 @@ void WorldModelPublisherComponent::publishWorldModel() Point p2; p1 << history.at(index).x, history.at(index).y; p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y; - visualizer->addLine(p1, p2, 1, "yellow", index / static_cast(history.size())); + // visualizer->addLine(p1, p2, 1, "yellow", index / static_cast(history.size())); + SvgLineBuilder line_builder; + line_builder.start(p1) + .end(p2) + .stroke("yellow", index / static_cast(history.size())) + .strokeWidth(3); + visualizer->add(line_builder.getSvgString()); } } } @@ -460,20 +465,32 @@ void WorldModelPublisherComponent::publishWorldModel() Point p2; p1 << history.at(index).x, history.at(index).y; p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y; - visualizer->addLine(p1, p2, 1, "blue", index / static_cast(history.size())); + // visualizer->addLine(p1, p2, 1, "blue", index / static_cast(history.size())); + SvgLineBuilder line_builder; + line_builder.start(p1) + .end(p2) + .stroke("blue", index / static_cast(history.size())) + .strokeWidth(3); + visualizer->add(line_builder.getSvgString()); } } } if (ball_history.size() > SAMPLING_NUM + 1) { for (int index = 0; index < ball_history.size() - SAMPLING_NUM; index += SAMPLING_NUM) { - visualizer->addLine( - ball_history.at(index), ball_history.at(index + SAMPLING_NUM), 1, "orange", - index / static_cast(ball_history.size())); + // visualizer->addLine( + // ball_history.at(index), ball_history.at(index + SAMPLING_NUM), 1, "orange", + // index / static_cast(ball_history.size())); + SvgLineBuilder line_builder; + line_builder.start(ball_history.at(index)) + .end(ball_history.at(index + SAMPLING_NUM)) + .stroke("orange", index / static_cast(ball_history.size())) + .strokeWidth(3); + visualizer->add(line_builder.getSvgString()); } } visualizer->flush(); - ConsaiVisualizerBuffer::publish(); + CraneVisualizerBuffer::publish(); } void WorldModelPublisherComponent::updateBallContact() diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp index d877513d7..89d1c6ad7 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp @@ -45,14 +45,32 @@ class AttackerSkillPlanner : public PlannerBase return {PlannerBase::Status::RUNNING, {}}; } else { std::string state_name(magic_enum::enum_name(skill->getCurrentState())); - visualizer->addCircle( - skill->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name); + { + // visualizer->addCircle( + // skill->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name); + SvgCircleBuilder circle_builder; + circle_builder.center(skill->commander().getRobot()->pose.pos) + .radius(0.3) + .stroke("red") + .strokeWidth(2); + visualizer->add(circle_builder.getSvgString()); + } if (world_model->ball.isMoving()) { - visualizer->addLine( - world_model->ball.pos, - world_model->ball.pos + - world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(), - 3, "red", 0.5, ""); + { + // visualizer->addLine( + // world_model->ball.pos, + // world_model->ball.pos + + // world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(), + // 3, "red", 0.5, ""); + SvgLineBuilder line_builder; + line_builder.start(world_model->ball.pos) + .end( + world_model->ball.pos + + world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon()) + .stroke("red") + .strokeWidth(3); + visualizer->add(line_builder.getSvgString()); + } } auto status = skill->run(); return {static_cast(status), {skill->getRobotCommand()}}; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/offensive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/offensive_planner.hpp index 3971b8ad1..e0c91f1bb 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/offensive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/offensive_planner.hpp @@ -42,13 +42,31 @@ class OffensivePlanner : public PlannerBase const std::vector & robots, PlannerContext & context) override { std::string state_name(magic_enum::enum_name(attacker->getCurrentState())); - visualizer->addCircle( - attacker->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name); - visualizer->addLine( - world_model->ball.pos, - world_model->ball.pos + - world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(), - 3, "red", 0.5, ""); + { + // visualizer->addCircle( + // attacker->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name); + SvgCircleBuilder circle_builder; + circle_builder.center(attacker->commander().getRobot()->pose.pos) + .radius(0.3) + .stroke("red") + .strokeWidth(2); + visualizer->add(circle_builder.getSvgString()); + } + { + // visualizer->addLine( + // world_model->ball.pos, + // world_model->ball.pos + + // world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(), + // 3, "red", 0.5, ""); + SvgLineBuilder line_builder; + line_builder.start(world_model->ball.pos) + .end( + world_model->ball.pos + + world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon()) + .stroke("red") + .strokeWidth(3); + visualizer->add(line_builder.getSvgString()); + } auto status = attacker->run(); return {static_cast(status), {attacker->getRobotCommand()}}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp index aeb8e9aba..03f2c53f3 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/placement_avoidance_planner.hpp @@ -95,7 +95,13 @@ class BallPlacementAvoidancePlanner : public PlannerBase // ボールプレイスメントエリアを横切ってしまうことがあるため、上書きしてしまう command.original_position = target_position; command.command->setTargetPosition(target_position); - visualizer->addLine(command.original_position, target_position, 2, "yellow"); + // visualizer->addLine(command.original_position, target_position, 2, "yellow"); + SvgLineBuilder line_builder; + line_builder.start(command.original_position) + .end(target_position) + .stroke("yellow") + .strokeWidth(2); + visualizer->add(line_builder.getSvgString()); } else { command.command->setTargetPosition(command.original_position); } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/planner_base.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/planner_base.hpp index beedc2514..d3516fe91 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planner_base.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planner_base.hpp @@ -10,7 +10,7 @@ #include #include #include -#include +#include #include #include #include @@ -50,7 +50,7 @@ class PlannerBase explicit PlannerBase(const std::string & name, WorldModelWrapper::SharedPtr & world_model) : name(name), world_model(world_model), - visualizer(std::make_unique("session_planner", name)) + visualizer(std::make_unique("session_planner/" + name)) { } @@ -178,7 +178,7 @@ class PlannerBase Status status = Status::RUNNING; - ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; private: std::vector> robot_select_callbacks; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp index f9526cb5e..b081761a9 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/simple_placer_planner.hpp @@ -189,13 +189,29 @@ class SimplePlacerPlanner : public PlannerBase // visualize areas with info for (const auto & area : areas_with_info) { - visualizer->addRect(area.box, 1., "yellow", "", 1., area.name); + // visualizer->addRect(area.box, 1., "yellow", "", 1., area.name); + SvgRectBuilder rect_builder; + rect_builder.box(area.box).stroke("yellow").strokeWidth(1); + visualizer->add(rect_builder.getSvgString()); + + SvgTextBuilder text_builder; + text_builder.position(area.box.min_corner().x(), area.box.min_corner().y()) + .text(area.name) + .fill("yellow") + .fontSize(100); + visualizer->add(text_builder.getSvgString()); } for (const auto & cmd : robot_commands) { - visualizer->addLine( - cmd.current_pose.x, cmd.current_pose.y, cmd.position_target_mode.front().target_x, - cmd.position_target_mode.front().target_y, 1, "blue"); + // visualizer->addLine( + // cmd.current_pose.x, cmd.current_pose.y, cmd.position_target_mode.front().target_x, + // cmd.position_target_mode.front().target_y, 1, "blue"); + SvgLineBuilder line_builder; + line_builder.start(cmd.current_pose.x, cmd.current_pose.y) + .end(cmd.position_target_mode.front().target_x, cmd.position_target_mode.front().target_y) + .stroke("blue") + .strokeWidth(1); + visualizer->add(line_builder.getSvgString()); } return {PlannerBase::Status::RUNNING, robot_commands}; } diff --git a/session/crane_session_controller/include/crane_session_controller/session_controller.hpp b/session/crane_session_controller/include/crane_session_controller/session_controller.hpp index b31543e8d..64ee1f0bd 100644 --- a/session/crane_session_controller/include/crane_session_controller/session_controller.hpp +++ b/session/crane_session_controller/include/crane_session_controller/session_controller.hpp @@ -8,7 +8,7 @@ #define CRANE_SESSION_CONTROLLER__SESSION_CONTROLLER_HPP_ #include -#include +#include #include #include #include diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index a633c3e70..6535dac53 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -26,7 +26,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions world_model(std::make_shared(*this)), robot_commands_pub(create_publisher("/control_targets", 1)) { - crane::ConsaiVisualizerBuffer::activate(*this); + crane::CraneVisualizerBuffer::activate(*this); world_model->setBallOwnerCalculatorEnabled(true); robot_roles = std::make_shared>(); @@ -217,7 +217,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions } msg.header.stamp = now(); robot_commands_pub->publish(msg); - ConsaiVisualizerBuffer::publish(); + CraneVisualizerBuffer::publish(); }); session_injection_sub = create_subscription( diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp deleted file mode 100644 index 3a0fa9ba6..000000000 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp +++ /dev/null @@ -1,903 +0,0 @@ -// Copyright (c) 2024 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#ifndef CRANE_MSG_WRAPPERS__CONSAI_VISUALIZER_WRAPPER_HPP_ -#define CRANE_MSG_WRAPPERS__CONSAI_VISUALIZER_WRAPPER_HPP_ - -#include -#include -#include -#include -#include - -namespace crane -{ - -template -struct ColorBuilder -{ - MsgT & msg; - explicit ColorBuilder(MsgT & msg) : msg(msg) - { - msg.color.name = "white"; - msg.color.alpha = 1.0; - } - - ColorBuilder & color(const std::string & name) - { - msg.color.name = name; - return *this; - } - - ColorBuilder & alpha(double a) - { - msg.color.alpha = a; - return *this; - } - - ColorBuilder & color(double r, double g, double b, double a = 1.0) - { - msg.color.red = r; - msg.color.green = g; - msg.color.blue = b; - msg.color.alpha = a; - return *this; - } -}; - -template -struct FillShapeColorBuilder -{ - MsgT & msg; - explicit FillShapeColorBuilder(MsgT & msg) : msg(msg) - { - msg.fill_color.name = "white"; - msg.fill_color.alpha = 0.0; - msg.line_color.name = "white"; - msg.line_color.alpha = 1.0; - } - - FillShapeColorBuilder & color(const std::string & name) - { - msg.fill_color.name = name; - msg.fill_color.alpha = 1.0; - msg.line_color.name = name; - msg.line_color.alpha = 1.0; - return *this; - } - - FillShapeColorBuilder & color(double r, double g, double b, double a = 1.0) - { - msg.fill_color.red = r; - msg.fill_color.green = g; - msg.fill_color.blue = b; - msg.fill_color.alpha = a; - msg.line_color.red = r; - msg.line_color.green = g; - msg.line_color.blue = b; - msg.line_color.alpha = a; - return *this; - } - - FillShapeColorBuilder & alpha(double a) - { - msg.fill_color.alpha = a; - msg.line_color.alpha = a; - return *this; - } - - FillShapeColorBuilder & fill_alpha(double a) - { - msg.fill_color.alpha = a; - return *this; - } - - FillShapeColorBuilder & line_alpha(double a) - { - msg.line_color.alpha = a; - return *this; - } - - FillShapeColorBuilder & fill_color(double r, double g, double b, double a = 1.0) - { - msg.fill_color.red = r; - msg.fill_color.green = g; - msg.fill_color.blue = b; - msg.fill_color.alpha = a; - return *this; - } - - FillShapeColorBuilder & line_color(double r, double g, double b, double a = 1.0) - { - msg.line_color.red = r; - msg.line_color.green = g; - msg.line_color.blue = b; - msg.line_color.alpha = a; - return *this; - } - - FillShapeColorBuilder & fill_color(const std::string & name) - { - msg.fill_color.name = name; - return *this; - } - - FillShapeColorBuilder & line_color(const std::string & name) - { - msg.line_color.name = name; - return *this; - } -}; - -struct ShapeAnnotationBuilder -: public ColorBuilder -{ - crane_visualization_interfaces::msg::ShapeAnnotation annotation; - ShapeAnnotationBuilder() - : ColorBuilder(annotation) - { - annotation.color.name = "white"; - annotation.color.alpha = 1.0; - } - operator crane_visualization_interfaces::msg::ShapeAnnotation() const { return annotation; } - - ShapeAnnotationBuilder & text(const std::string & text) - { - annotation.text = text; - return *this; - } - - ShapeAnnotationBuilder & normalized_x(double normalized_x) - { - annotation.normalized_x = normalized_x; - return *this; - } - - ShapeAnnotationBuilder & normalized_y(double normalized_y) - { - annotation.normalized_y = normalized_y; - return *this; - } - - ShapeAnnotationBuilder & normalized_position(Point p) - { - annotation.normalized_x = p.x(); - annotation.normalized_y = p.y(); - return *this; - } - - ShapeAnnotationBuilder & normalized_width(double normalized_width) - { - annotation.normalized_width = normalized_width; - return *this; - } - - ShapeAnnotationBuilder & normalized_height(double normalized_height) - { - annotation.normalized_height = normalized_height; - return *this; - } -}; - -struct ShapePointBuilder : public ColorBuilder -{ - crane_visualization_interfaces::msg::ShapePoint point; - ShapePointBuilder() : ColorBuilder(point) - { - point.color.name = "white"; - point.color.alpha = 1.0; - point.size = 1; - } - operator crane_visualization_interfaces::msg::ShapePoint() const { return point; } - - ShapePointBuilder & x(double x) - { - point.x = x; - return *this; - } - - ShapePointBuilder & y(double y) - { - point.y = y; - return *this; - } - - ShapePointBuilder & position(Point p) - { - point.x = p.x(); - point.y = p.y(); - return *this; - } - - ShapePointBuilder & size(int size) - { - point.size = size; - return *this; - } - - ShapePointBuilder & caption(const std::string & caption) - { - point.caption = caption; - return *this; - } -}; - -struct ShapeArcBuilder : public ColorBuilder -{ - crane_visualization_interfaces::msg::ShapeArc arc; - ShapeArcBuilder() : ColorBuilder(arc) {} - operator crane_visualization_interfaces::msg::ShapeArc() const { return arc; } - - ShapeArcBuilder & center(double x, double y) - { - arc.center.x = x; - arc.center.y = y; - return *this; - } - - ShapeArcBuilder & center(Point p) - { - arc.center.x = p.x(); - arc.center.y = p.y(); - return *this; - } - - ShapeArcBuilder & radius(double radius) - { - arc.radius = radius; - return *this; - } - - ShapeArcBuilder & start_angle(double start_angle) - { - arc.start_angle = start_angle; - return *this; - } - - ShapeArcBuilder & end_angle(double end_angle) - { - arc.end_angle = end_angle; - return *this; - } - - ShapeArcBuilder & angle_range(double start, double end) - { - arc.start_angle = start; - arc.end_angle = end; - return *this; - } - ShapeArcBuilder & size(int size) - { - arc.size = size; - return *this; - } - - ShapeArcBuilder & caption(const std::string & caption) - { - arc.caption = caption; - return *this; - } -}; - -struct ShapeCircleBuilder -: public FillShapeColorBuilder -{ - crane_visualization_interfaces::msg::ShapeCircle circle; - ShapeCircleBuilder() - : FillShapeColorBuilder(circle) - { - circle.line_size = 1; - } - operator crane_visualization_interfaces::msg::ShapeCircle() const { return circle; } - - ShapeCircleBuilder & center(double x, double y) - { - circle.center.x = x; - circle.center.y = y; - return *this; - } - - ShapeCircleBuilder & center(Point p) - { - circle.center.x = p.x(); - circle.center.y = p.y(); - return *this; - } - - ShapeCircleBuilder & radius(double radius) - { - circle.radius = radius; - return *this; - } - - ShapeCircleBuilder & line_size(int line_size) - { - circle.line_size = line_size; - return *this; - } - - ShapeCircleBuilder & caption(const std::string & caption) - { - circle.caption = caption; - return *this; - } -}; - -struct ShapeLineBuilder : public ColorBuilder -{ - crane_visualization_interfaces::msg::ShapeLine line; - ShapeLineBuilder() : ColorBuilder(line) - { - line.size = 1; - } - - operator crane_visualization_interfaces::msg::ShapeLine() const { return line; } - - ShapeLineBuilder & p1(double x, double y) - { - line.p1.x = x; - line.p1.y = y; - return *this; - } - - ShapeLineBuilder & p1(Point p) - { - line.p1.x = p.x(); - line.p1.y = p.y(); - return *this; - } - - ShapeLineBuilder & p2(double x, double y) - { - line.p2.x = x; - line.p2.y = y; - return *this; - } - - ShapeLineBuilder & p2(Point p) - { - line.p2.x = p.x(); - line.p2.y = p.y(); - return *this; - } - - ShapeLineBuilder & size(int size) - { - line.size = size; - return *this; - } - - ShapeLineBuilder & caption(const std::string & caption) - { - line.caption = caption; - return *this; - } -}; - -struct ShapeRectangleBuilder -: public FillShapeColorBuilder -{ - crane_visualization_interfaces::msg::ShapeRectangle rect; - ShapeRectangleBuilder() - : FillShapeColorBuilder(rect) - { - } - - operator crane_visualization_interfaces::msg::ShapeRectangle() const { return rect; } - - ShapeRectangleBuilder & center(double x, double y) - { - rect.center.x = x; - rect.center.y = y; - return *this; - } - - ShapeRectangleBuilder & center(Point p) - { - rect.center.x = p.x(); - rect.center.y = p.y(); - return *this; - } - - ShapeRectangleBuilder & size(double x, double y) - { - rect.width = x; - rect.height = y; - return *this; - } - - ShapeRectangleBuilder & size(Point p) - { - rect.width = p.x(); - rect.height = p.y(); - return *this; - } - - ShapeRectangleBuilder & line_size(int line_size) - { - rect.line_size = line_size; - return *this; - } - - ShapeRectangleBuilder & caption(const std::string & caption) - { - rect.caption = caption; - return *this; - } -}; - -struct ShapeRobotBuilder -: public FillShapeColorBuilder -{ - crane_visualization_interfaces::msg::ShapeRobot robot; - ShapeRobotBuilder() - : FillShapeColorBuilder(robot) - { - robot.radius = 0.09; - robot.line_size = 1; - } - operator crane_visualization_interfaces::msg::ShapeRobot() const { return robot; } - - ShapeRobotBuilder & x(double x) - { - robot.x = x; - return *this; - } - - ShapeRobotBuilder & y(double y) - { - robot.y = y; - return *this; - } - - ShapeRobotBuilder & position(Point p) - { - robot.x = p.x(); - robot.y = p.y(); - return *this; - } - - ShapeRobotBuilder & theta(double theta) - { - robot.theta = theta; - return *this; - } - - ShapeRobotBuilder & id(double id) - { - robot.id = id; - return *this; - } - - ShapeRobotBuilder & color_type(bool color_type) - { - robot.color_type.value = color_type; - return *this; - } - - ShapeRobotBuilder & color_type_default() - { - robot.color_type.value = 0; - return *this; - } - - ShapeRobotBuilder & color_type_real() - { - robot.color_type.value = 1; - return *this; - } - - ShapeRobotBuilder & radius(double radius) - { - robot.radius = radius; - return *this; - } - - ShapeRobotBuilder & line_size(int line_size) - { - robot.line_size = line_size; - return *this; - } - - ShapeRobotBuilder & caption(const std::string & caption) - { - robot.caption = caption; - return *this; - } -}; - -struct ShapeTubeBuilder -: public FillShapeColorBuilder -{ - crane_visualization_interfaces::msg::ShapeTube tube; - ShapeTubeBuilder() : FillShapeColorBuilder(tube) - { - tube.line_size = 1; - } - operator crane_visualization_interfaces::msg::ShapeTube() const { return tube; } - - ShapeTubeBuilder & p1(double x, double y) - { - tube.p1.x = x; - tube.p1.y = y; - return *this; - } - - ShapeTubeBuilder & p1(Point p) - { - tube.p1.x = p.x(); - tube.p1.y = p.y(); - return *this; - } - - ShapeTubeBuilder & p2(double x, double y) - { - tube.p2.x = x; - tube.p2.y = y; - return *this; - } - - ShapeTubeBuilder & p2(Point p) - { - tube.p2.x = p.x(); - tube.p2.y = p.y(); - return *this; - } - - ShapeTubeBuilder & radius(double radius) - { - tube.radius = radius; - return *this; - } - - ShapeTubeBuilder & line_size(int line_size) - { - tube.line_size = line_size; - return *this; - } - - ShapeTubeBuilder & caption(const std::string & caption) - { - tube.caption = caption; - return *this; - } -}; - -struct ConsaiVisualizerBuffer -{ - using ObjectsArray = crane_visualization_interfaces::msg::ObjectsArray; - static inline std::unique_ptr buffer = nullptr; - - rclcpp::Publisher::SharedPtr publisher; - - crane_visualization_interfaces::msg::ObjectsArray message_buffer; - - template - ConsaiVisualizerBuffer(Node & node, const std::string topic) - { - publisher = node.template create_publisher(topic, rclcpp::SensorDataQoS()); - } - - template - static auto activate(Node & node, const std::string & topic = "/visualizer_objects") -> void - { - if (not active()) { - buffer = std::make_unique(node, topic); - } - } - - static auto deactivate() -> void - { - if (active()) { - buffer.reset(); - } - } - - static auto active() -> bool { return buffer != nullptr; } - - static auto publish() -> void - { - if (active()) { - buffer->publisher->publish(buffer->message_buffer); - buffer->message_buffer.objects.clear(); - } - } - - struct MessageBuilder - { - using SharedPtr = std::shared_ptr; - using UniquePtr = std::unique_ptr; - - explicit MessageBuilder(const std::string & layer, const std::string & sub_layer = "default") - { - message_buffer.layer = layer; - message_buffer.sub_layer = sub_layer; - } - - void clear() - { - message_buffer.annotations.clear(); - message_buffer.points.clear(); - message_buffer.lines.clear(); - message_buffer.arcs.clear(); - message_buffer.rects.clear(); - message_buffer.circles.clear(); - message_buffer.tubes.clear(); - message_buffer.texts.clear(); - message_buffer.robots.clear(); - } - - void flush() - { - if (ConsaiVisualizerBuffer::active()) { - ConsaiVisualizerBuffer::buffer->message_buffer.objects.push_back(message_buffer); - clear(); - } - } - - using Objects = crane_visualization_interfaces::msg::Objects; - - Objects message_buffer; - - void addAnnotation( - const std::string & text, double normed_x, double normed_y, double normed_width, - double normed_height, const std::string & color = "white", double alpha = 1.0) - { - crane_visualization_interfaces::msg::ShapeAnnotation annotation; - annotation.text = text; - annotation.normalized_x = normed_x; - annotation.normalized_y = normed_y; - annotation.normalized_width = normed_width; - annotation.normalized_height = normed_height; - annotation.color.name = color; - annotation.color.alpha = alpha; - message_buffer.annotations.push_back(annotation); - } - - void addAnnotation(crane_visualization_interfaces::msg::ShapeAnnotation annotation) - { - message_buffer.annotations.push_back(annotation); - } - - void addPoint( - double x, double y, int size, const std::string & color = "white", double alpha = 1.0, - const std::string & caption = "") - { - crane_visualization_interfaces::msg::ShapePoint point; - point.x = x; - point.y = y; - point.color.name = color; - point.color.alpha = alpha; - point.size = size; - point.caption = caption; - message_buffer.points.push_back(point); - } - - void addPoint( - Point p, int size, const std::string & color = "white", double alpha = 1.0, - const std::string & caption = "") - { - addPoint(p.x(), p.y(), size, color, alpha, caption); - } - - void addPoint(crane_visualization_interfaces::msg::ShapePoint point) - { - message_buffer.points.push_back(point); - } - - void addLine( - double x1, double y1, double x2, double y2, int size, const std::string & color = "white", - double alpha = 1.0, const std::string & caption = "") - { - crane_visualization_interfaces::msg::ShapeLine line; - line.p1.x = x1; - line.p1.y = y1; - line.p2.x = x2; - line.p2.y = y2; - line.color.name = color; - line.color.alpha = alpha; - line.size = size; - line.caption = caption; - message_buffer.lines.push_back(line); - } - - void addLine( - Point p1, Point p2, int size, const std::string & color = "white", double alpha = 1.0, - const std::string & caption = "") - { - addLine(p1.x(), p1.y(), p2.x(), p2.y(), size, color, alpha, caption); - } - - void addLine(crane_visualization_interfaces::msg::ShapeLine line) - { - message_buffer.lines.push_back(line); - } - - void addArc( - double x, double y, double radius, double start_angle, double end_angle, int size, - const std::string & color = "white", double alpha = 1.0, const std::string & caption = "") - { - crane_visualization_interfaces::msg::ShapeArc arc; - arc.center.x = x; - arc.center.y = y; - arc.radius = radius; - arc.start_angle = start_angle; - arc.end_angle = end_angle; - arc.color.name = color; - arc.color.alpha = alpha; - arc.size = size; - arc.caption = caption; - message_buffer.arcs.push_back(arc); - } - - void addArc( - Point center, double radius, double start_angle, double end_angle, int size, - const std::string & color = "white", double alpha = 1.0, const std::string & caption = "") - { - addArc(center.x(), center.y(), radius, start_angle, end_angle, size, color, alpha, caption); - } - - void addArc(crane_visualization_interfaces::msg::ShapeArc arc) - { - message_buffer.arcs.push_back(arc); - } - - void addRect( - double x, double y, double width, double height, int line_size, - const std::string & line_color = "white", const std::string & fill_color = "white", - double alpha = 1.0, const std::string & caption = "") - { - crane_visualization_interfaces::msg::ShapeRectangle rect; - rect.center.x = x; - rect.center.y = y; - rect.width = width; - rect.height = height; - rect.line_color.name = line_color; - rect.line_color.alpha = alpha; - rect.fill_color.name = fill_color; - if (fill_color == "") { - rect.fill_color.alpha = 0.0; - } else { - rect.fill_color.alpha = alpha; - } - rect.line_size = line_size; - rect.caption = caption; - message_buffer.rects.push_back(rect); - } - - void addRect( - Point center, double width, double height, int line_size, - const std::string & line_color = "white", const std::string & fill_color = "white", - double alpha = 1.0, const std::string & caption = "") - { - addRect( - center.x(), center.y(), width, height, line_size, line_color, fill_color, alpha, caption); - } - - void addRect( - const Box & box, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, - const std::string & caption = "") - { - Point center; - bg::centroid(box, center); - const double width = box.max_corner().x() - box.min_corner().x(); - const double height = box.max_corner().y() - box.min_corner().y(); - addRect(center, width, height, line_size, line_color, fill_color, alpha, caption); - } - - void addRect(crane_visualization_interfaces::msg::ShapeRectangle rect) - { - message_buffer.rects.push_back(rect); - } - - void addCircle( - double x, double y, double radius, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, - const std::string & caption = "") - { - crane_visualization_interfaces::msg::ShapeCircle circle; - circle.center.x = x; - circle.center.y = y; - circle.radius = radius; - circle.line_color.name = line_color; - circle.line_color.alpha = alpha; - circle.fill_color.name = fill_color; - if (fill_color == "") { - circle.fill_color.alpha = 0.0; - } else { - circle.fill_color.alpha = alpha; - } - circle.line_size = line_size; - circle.caption = caption; - message_buffer.circles.push_back(circle); - } - - void addCircle( - Point center, double radius, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, - const std::string & caption = "") - { - addCircle(center.x(), center.y(), radius, line_size, line_color, fill_color, alpha, caption); - } - - void addCircle(crane_visualization_interfaces::msg::ShapeCircle circle) - { - message_buffer.circles.push_back(circle); - } - - void addTube( - double x1, double y1, double x2, double y2, double radius, int line_size, - const std::string & line_color = "white", const std::string & fill_color = "white", - double alpha = 1.0, const std::string & caption = "") - { - crane_visualization_interfaces::msg::ShapeTube tube; - tube.p1.x = x1; - tube.p1.y = y1; - tube.p2.x = x2; - tube.p2.y = y2; - tube.radius = radius; - tube.line_color.name = line_color; - tube.line_color.alpha = alpha; - tube.fill_color.name = fill_color; - tube.fill_color.alpha = alpha; - tube.line_size = line_size; - tube.caption = caption; - message_buffer.tubes.push_back(tube); - } - - void addTube( - Point p1, Point p2, double radius, int line_size, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, - const std::string & caption = "") - { - addTube( - p1.x(), p1.y(), p2.x(), p2.y(), radius, line_size, line_color, fill_color, alpha, caption); - } - - void addTube(crane_visualization_interfaces::msg::ShapeTube tube) - { - message_buffer.tubes.push_back(tube); - } - - void addRobot( - double id, double x, double y, double theta, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, double line_size = 1, - const std::string & caption = "", bool color_type = false) - { - crane_visualization_interfaces::msg::ShapeRobot robot; - robot.x = x; - robot.y = y; - robot.theta = theta; - robot.radius = 0.09; - robot.line_color.name = line_color; - robot.line_color.alpha = alpha; - robot.fill_color.name = fill_color; - robot.fill_color.alpha = alpha; - robot.line_size = line_size; - robot.caption = caption; - robot.id = id; - robot.color_type.value = color_type; - message_buffer.robots.push_back(robot); - } - - void addRobot( - double id, Point p, double theta, const std::string & line_color = "white", - const std::string & fill_color = "white", double alpha = 1.0, double line_size = 1, - const std::string & caption = "", bool color_type = false) - { - addRobot( - id, p.x(), p.y(), theta, line_color, fill_color, alpha, line_size, caption, color_type); - } - - void addRobot(crane_visualization_interfaces::msg::ShapeRobot robot) - { - message_buffer.robots.push_back(robot); - } - }; -}; -} // namespace crane -#endif // CRANE_MSG_WRAPPERS__CONSAI_VISUALIZER_WRAPPER_HPP_ diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp new file mode 100644 index 000000000..172069f43 --- /dev/null +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/crane_visualizer_wrapper.hpp @@ -0,0 +1,636 @@ +// Copyright (c) 2025 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_MSG_WRAPPERS__CRANE_VISUALIZER_WRAPPER_HPP_ +#define CRANE_MSG_WRAPPERS__CRANE_VISUALIZER_WRAPPER_HPP_ + +#include +#include +#include +#include +#include + +namespace crane +{ +struct SvgCircleBuilder +{ + Point circle_center; + + double circle_radius; + + std::string fill_color = "none"; + + double fill_opacity = 1.; + + std::string stroke_color = "black"; + + double stroke_opacity = 1.; + + double stroke_width = 1.0; + + SvgCircleBuilder() {} + + std::string getSvgString() const + { + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgCircleBuilder & center(double x, double y) + { + circle_center = Point(x, y); + return *this; + } + + SvgCircleBuilder & center(Point p) + { + circle_center = p; + return *this; + } + + SvgCircleBuilder & radius(double radius) + { + circle_radius = radius; + return *this; + } + + SvgCircleBuilder & fill(const std::string & color, double alpha = 1.0) + { + fill_color = color; + fill_opacity = alpha; + return *this; + } + + SvgCircleBuilder & stroke(const std::string & color, double alpha = 1.0) + { + stroke_color = color; + stroke_opacity = alpha; + return *this; + } + + SvgCircleBuilder & strokeWidth(double width) + { + stroke_width = width; + return *this; + } +}; + +struct SvgLineBuilder +{ + Point p1; + Point p2; + + std::string stroke_color = "black"; + + double stroke_opacity = 1.; + + double stroke_width = 1.0; + + SvgLineBuilder() {} + + std::string getSvgString() const + { + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgLineBuilder & start(double x, double y) { return start(Point(x, y)); } + + SvgLineBuilder & start(Point p) + { + p1 = p; + return *this; + } + + SvgLineBuilder & end(double x, double y) { return end(Point(x, y)); } + + SvgLineBuilder & end(Point p) + { + p2 = p; + return *this; + } + + SvgLineBuilder & stroke(const std::string & color, double alpha = 1.0) + { + stroke_color = color; + stroke_opacity = alpha; + return *this; + } + + SvgLineBuilder & strokeWidth(double width) + { + stroke_width = width; + return *this; + } +}; + +struct SvgRectBuilder +{ + Point rect_top_left; + + Point rect_size; + + std::string fill_color = "none"; + + double fill_opacity = 1.; + + std::string stroke_color = "black"; + + double stroke_opacity = 1.; + + double stroke_width = 1.0; + + SvgRectBuilder() {} + + std::string getSvgString() const + { + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgRectBuilder & top_left(double x, double y) + { + rect_top_left = Point(x, y); + return *this; + } + + SvgRectBuilder & top_left(Point p) + { + rect_top_left = p; + return *this; + } + + SvgRectBuilder & size(double x, double y) + { + rect_size = Point(x, y); + return *this; + } + + SvgRectBuilder & size(Point p) + { + rect_size = p; + return *this; + } + + SvgRectBuilder & box(const Box & box) + { + rect_top_left = box.min_corner(); + rect_size = box.max_corner() - box.min_corner(); + return *this; + } + + SvgRectBuilder & fill(const std::string & color, double alpha = 1.0) + { + fill_color = color; + fill_opacity = alpha; + return *this; + } + + SvgRectBuilder & stroke(const std::string & color, double alpha = 1.0) + { + stroke_color = color; + stroke_opacity = alpha; + return *this; + } + + SvgRectBuilder & strokeWidth(double width) + { + stroke_width = width; + return *this; + } +}; + +struct SvgTextBuilder +{ + Point text_position; + + std::string text_string; + + std::string fill_color = "white"; + + double fill_opacity = 1.; + + double font_size = 100.0; + + bool view_box_position = false; + + std::string anchor = "start"; + + SvgTextBuilder() {} + + std::string getSvgString() const + { + std::ostringstream oss; + oss << "" << text_string << ""; + return oss.str(); + } + + SvgTextBuilder & position(double x, double y) { return position(Point(x, y)); } + + SvgTextBuilder & position(Point p) + { + view_box_position = false; + text_position = p; + return *this; + } + + SvgTextBuilder & viewBoxPosition(double x, double y) { return viewBoxPosition(Point(x, y)); } + + SvgTextBuilder & viewBoxPosition(Point p) + { + view_box_position = true; + text_position = p; + return *this; + } + + SvgTextBuilder & text(const std::string & text) + { + this->text_string = text; + return *this; + } + + SvgTextBuilder & fill(const std::string & color, double alpha = 1.0) + { + fill_color = color; + fill_opacity = alpha; + return *this; + } + + SvgTextBuilder & fontSize(double size) + { + font_size = size; + return *this; + } + + SvgTextBuilder & textAnchor(const std::string & anchor) + { + this->anchor = anchor; + return *this; + } +}; + +struct SvgPolyLineBuilder +{ + std::vector points; + + std::string stroke_color = "black"; + + double stroke_opacity = 1.; + + double stroke_width = 1.0; + + SvgPolyLineBuilder() {} + + std::string getSvgString() const + { + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgPolyLineBuilder & addPoint(double x, double y) + { + points.emplace_back(x, y); + return *this; + } + + SvgPolyLineBuilder & addPoint(Point p) + { + points.push_back(p); + return *this; + } + + SvgPolyLineBuilder & strokeColor(const std::string & color, double alpha = 1.0) + { + stroke_color = color; + stroke_opacity = alpha; + return *this; + } + + SvgPolyLineBuilder & strokeWidth(double width) + { + stroke_width = width; + return *this; + } +}; + +struct SvgPolygonBuilder +{ + std::vector points; + + std::string fill_color = "none"; + + double fill_opacity = 1.; + + std::string stroke_color = "black"; + + double stroke_opacity = 1.; + + double stroke_width = 1.0; + + SvgPolygonBuilder() {} + + std::string getSvgString() const + { + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgPolygonBuilder & addPoint(double x, double y) + { + points.emplace_back(x, y); + return *this; + } + + SvgPolygonBuilder & addPoint(Point p) + { + points.push_back(p); + return *this; + } + + SvgPolygonBuilder & fill(const std::string & color, double alpha = 1.0) + { + fill_color = color; + fill_opacity = alpha; + return *this; + } + + SvgPolygonBuilder & stroke(const std::string & color, double alpha = 1.0) + { + stroke_color = color; + stroke_opacity = alpha; + return *this; + } + + SvgPolygonBuilder & strokeWidth(double width) + { + stroke_width = width; + return *this; + } +}; + +struct SvgPathBuilder +{ + std::string path; + + std::string fill_color = "none"; + + double fill_opacity = 1.; + + std::string stroke_color = "black"; + + double stroke_opacity = 1.; + + double stroke_width = 1.0; + + SvgPathBuilder() {} + + virtual std::string getSvgString() const + { + std::ostringstream oss; + oss << ""; + return oss.str(); + } + + SvgPathBuilder & pathString(const std::string & path) + { + this->path = path; + return *this; + } + + SvgPathBuilder & fill(const std::string & color, double alpha = 1.0) + { + fill_color = color; + fill_opacity = alpha; + return *this; + } + + SvgPathBuilder & stroke(const std::string & color, double alpha = 1.0) + { + stroke_color = color; + stroke_opacity = alpha; + return *this; + } + + SvgPathBuilder & strokeWidth(double width) + { + stroke_width = width; + return *this; + } + + struct SvgPathDefinitionBuilder + { + std::string path; + + SvgPathDefinitionBuilder & moveTo(double x, double y) + { + path += " M" + std::to_string(x * 1000.) + "," + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & moveTo(Point p) { return moveTo(p.x(), p.y()); } + + SvgPathDefinitionBuilder & lineTo(double x, double y) + { + path += " L" + std::to_string(x * 1000.) + "," + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & lineTo(Point p) { return lineTo(p.x(), p.y()); } + + SvgPathDefinitionBuilder & horizontalTo(double x) + { + path += " H" + std::to_string(x * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & verticalTo(double y) + { + path += " V" + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & closePath() + { + path += " Z"; + return *this; + } + + SvgPathDefinitionBuilder & cubicBezierTo( + double x1, double y1, double x2, double y2, double x, double y) + { + path += " C" + std::to_string(x1 * 1000.) + "," + std::to_string(y1 * 1000.) + " " + + std::to_string(x2 * 1000.) + "," + std::to_string(y2 * 1000.) + " " + + std::to_string(x * 1000.) + "," + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & cubicBezierTo(Point p1, Point p2, Point p) + { + return cubicBezierTo(p1.x(), p1.y(), p2.x(), p2.y(), p.x(), p.y()); + } + + SvgPathDefinitionBuilder & smoothCubicBezierTo(double x2, double y2, double x, double y) + { + path += " S" + std::to_string(x2 * 1000.) + "," + std::to_string(y2 * 1000.) + " " + + std::to_string(x * 1000.) + "," + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & smoothCubicBezierTo(Point p2, Point p) + { + return smoothCubicBezierTo(p2.x(), p2.y(), p.x(), p.y()); + } + + SvgPathDefinitionBuilder & quadraticBezierTo(double x1, double y1, double x, double y) + { + path += " Q" + std::to_string(x1 * 1000.) + "," + std::to_string(y1 * 1000.) + " " + + std::to_string(x * 1000.) + "," + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & quadraticBezierTo(Point p1, Point p) + { + return quadraticBezierTo(p1.x(), p1.y(), p.x(), p.y()); + } + + SvgPathDefinitionBuilder & smoothQuadraticBezierTo(double x, double y) + { + path += " T" + std::to_string(x * 1000.) + "," + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & smoothQuadraticBezierTo(Point p) + { + return smoothQuadraticBezierTo(p.x(), p.y()); + } + + SvgPathDefinitionBuilder & arcTo( + double rx, double ry, double x_axis_rotation, bool large_arc_flag, bool sweep_flag, double x, + double y) + { + path += " A" + std::to_string(rx * 1000.) + "," + std::to_string(ry * 1000.) + " " + + std::to_string(x_axis_rotation) + " " + std::to_string(large_arc_flag) + "," + + std::to_string(sweep_flag) + " " + std::to_string(x * 1000.) + "," + + std::to_string(y * 1000.); + return *this; + } + + SvgPathDefinitionBuilder & arcTo( + Point r, double x_axis_rotation, bool large_arc_flag, bool sweep_flag, Point p) + { + return arcTo(r.x(), r.y(), x_axis_rotation, large_arc_flag, sweep_flag, p.x(), p.y()); + } + + SvgPathBuilder build() + { + SvgPathBuilder builder; + builder.path = path; + return builder; + } + }; +}; + +struct CraneVisualizerBuffer +{ + using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray; + static inline std::unique_ptr buffer = nullptr; + + rclcpp::Publisher::SharedPtr publisher; + + SvgPrimitiveArray message_buffer; + + template + CraneVisualizerBuffer(Node & node, const std::string topic) + { + publisher = node.template create_publisher(topic, rclcpp::SensorDataQoS()); + } + + template + static auto activate(Node & node, const std::string & topic = "/visualizer_svgs") -> void + { + if (not active()) { + buffer = std::make_unique(node, topic); + } + } + + static auto deactivate() -> void + { + if (active()) { + buffer.reset(); + } + } + + static auto active() -> bool { return buffer != nullptr; } + + static auto publish() -> void + { + if (active()) { + buffer->publisher->publish(buffer->message_buffer); + buffer->message_buffer.svg_primitives.clear(); + } + } + + struct MessageBuilder + { + using SharedPtr = std::shared_ptr; + using UniquePtr = std::unique_ptr; + + std::string layer; + + explicit MessageBuilder(const std::string & layer) : layer(layer) {} + + void flush() + { + if (CraneVisualizerBuffer::active()) { + CraneVisualizerBuffer::buffer->message_buffer.layer = layer; + CraneVisualizerBuffer::buffer->message_buffer.svg_primitives.insert( + CraneVisualizerBuffer::buffer->message_buffer.svg_primitives.end(), + message_buffer.begin(), message_buffer.end()); + message_buffer.clear(); + } + } + + using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray; + + std::vector message_buffer; + + void add(const std::string & svg_string) { message_buffer.push_back(svg_string); } + }; +}; +} // namespace crane +#endif // CRANE_MSG_WRAPPERS__CRANE_VISUALIZER_WRAPPER_HPP_ diff --git a/utility/crane_visualization_aggregator/.clang-format b/utility/crane_visualization_aggregator/.clang-format new file mode 100755 index 000000000..1db9fb9f5 --- /dev/null +++ b/utility/crane_visualization_aggregator/.clang-format @@ -0,0 +1,17 @@ +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: false diff --git a/utility/crane_visualization_aggregator/CMakeLists.txt b/utility/crane_visualization_aggregator/CMakeLists.txt new file mode 100755 index 000000000..775577816 --- /dev/null +++ b/utility/crane_visualization_aggregator/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.5) +project(crane_visualization_aggregator) + +# Default to C++20 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 20) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -g) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +ament_auto_add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/utility/crane_visualization_aggregator/package.xml b/utility/crane_visualization_aggregator/package.xml new file mode 100755 index 000000000..8876d0b2f --- /dev/null +++ b/utility/crane_visualization_aggregator/package.xml @@ -0,0 +1,23 @@ + + + + crane_visualization_aggregator + 0.0.0 + crane_visualization_aggregator + ibis ssl + MIT + + ament_cmake_auto + + crane_msgs + crane_visualization_interfaces + rclcpp + std_msgs + + ament_lint_auto + crane_lint_common + + + ament_cmake + + diff --git a/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp new file mode 100644 index 000000000..81021ded1 --- /dev/null +++ b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp @@ -0,0 +1,62 @@ +// Copyright (c) 2025 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#include +#include +#include +#include +#include +#include +#include + +/** crane_visualization_interfaces/msg/SvgPrimitiveArray.msg +string layer +string[] svg_primitives + */ +class VisualizationAggregator : public rclcpp::Node +{ +public: + VisualizationAggregator() : Node("visualization_aggregator") + { + subscriber = create_subscription( + "/visualizer_svgs", rclcpp::SensorDataQoS(), + [&](const crane_visualization_interfaces::msg::SvgPrimitiveArray::ConstSharedPtr & msg) { + // store into　layers + layers[msg->layer] = msg->svg_primitives; + }); + publisher = + create_publisher("/aggregated_svgs", 10); + timer = create_wall_timer(std::chrono::milliseconds(100), [this]() { + crane_visualization_interfaces::msg::SvgLayerArray msg; + + for (const auto & [layer, primitives] : layers) { + crane_visualization_interfaces::msg::SvgPrimitiveArray layer_msg; + layer_msg.layer = layer; + layer_msg.svg_primitives = primitives; + msg.svg_primitive_arrays.push_back(layer_msg); + } + publisher->publish(msg); + }); + } + +private: + rclcpp::Subscription::SharedPtr + subscriber; + rclcpp::Publisher::SharedPtr publisher; + + std::unordered_map> layers; + + rclcpp::TimerBase::SharedPtr timer; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}