From 03a301703951cacc7cdd8daa3247c4f64011f927 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 03:39:29 +0900 Subject: [PATCH 1/8] =?UTF-8?q?SvgPolyLineBuilder=E3=82=92=E8=AA=BF?= =?UTF-8?q?=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_msg_wrappers/crane_visualizer_wrapper.hpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) 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 index 172069f43..00760cf00 100644 --- 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 @@ -311,7 +311,11 @@ struct SvgPolyLineBuilder for (const auto & p : points) { oss << p.x() * 1000. << "," << p.y() * 1000. << " "; } - oss << "\" stroke=\"" << stroke_color << "\" stroke-width=\"" << stroke_width << "\" />"; + oss << "\" stroke=\"" << stroke_color << "\" stroke-width=\"" << stroke_width; + if (stroke_opacity != 1.) { + oss << "\" stroke-opacity=\"" << stroke_opacity; + } + oss << "\" fill=\"none\" />"; return oss.str(); } @@ -327,7 +331,7 @@ struct SvgPolyLineBuilder return *this; } - SvgPolyLineBuilder & strokeColor(const std::string & color, double alpha = 1.0) + SvgPolyLineBuilder & stroke(const std::string & color, double alpha = 1.0) { stroke_color = color; stroke_opacity = alpha; From 3cf2f3daa044f3cfec0d01be27d4fff1c3d0e7ee Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 03:39:49 +0900 Subject: [PATCH 2/8] =?UTF-8?q?SvgPathBuilder=E3=82=92=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_visualizer_wrapper.hpp | 21 +++---------------- 1 file changed, 3 insertions(+), 18 deletions(-) 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 index 00760cf00..345dd1e84 100644 --- 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 @@ -408,8 +408,6 @@ struct SvgPolygonBuilder struct SvgPathBuilder { - std::string path; - std::string fill_color = "none"; double fill_opacity = 1.; @@ -425,17 +423,11 @@ struct SvgPathBuilder virtual std::string getSvgString() const { std::ostringstream oss; - 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; @@ -559,14 +551,7 @@ struct SvgPathBuilder { 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; - } - }; + } definition; }; struct CraneVisualizerBuffer From aface29c9cbb1c4c8ea31510e0d60bbf88761d93 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 03:40:13 +0900 Subject: [PATCH 3/8] =?UTF-8?q?=E8=AA=BF=E6=95=B4=E5=BE=8C=E3=81=AESvgPath?= =?UTF-8?q?Builder=E3=82=92=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/visualization_data_handler.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp index c2d841df0..a6d005f90 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -27,24 +27,19 @@ struct SvgRobotBuilder : public SvgPathBuilder std::string getSvgString() const override { - SvgPathDefinitionBuilder path_builder; - path_builder + SvgPathBuilder path_builder; + path_builder.definition .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) + path_builder.fill(fill_color, fill_opacity) .stroke(stroke_color, stroke_opacity) .strokeWidth(stroke_width); - std::ostringstream oss; - oss << ""; - return oss.str(); + return path_builder.getSvgString(); } SvgRobotBuilder & position(Point p, double theta) From fb169895fe9afda4235e1da05a1b3b93827424a4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 03:40:31 +0900 Subject: [PATCH 4/8] =?UTF-8?q?=E3=83=80=E3=82=A4=E3=82=A8=E3=83=83?= =?UTF-8?q?=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/world_model_publisher.cpp | 62 +++++++++++-------- 1 file changed, 36 insertions(+), 26 deletions(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index a73bfde8c..ee7a7c5ba 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -442,46 +442,56 @@ void WorldModelPublisherComponent::publishWorldModel() constexpr int SAMPLING_NUM = 4; for (const auto & history : friend_history) { if (history.size() > SAMPLING_NUM + 1) { - for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) { - Point p1; - Point p2; - p1 << history.at(index).x, history.at(index).y; - p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y; - SvgLineBuilder line_builder; - line_builder.start(p1) - .end(p2) - .stroke("yellow", index / static_cast(history.size())) + for (int i = 0; i < 10; i++) { + SvgPolyLineBuilder polyline_builder; + int start = static_cast((history.size() / 10.) * i); + int end = static_cast((history.size() / 10.) * (i + 1)); + for (int index = start; index < end; index += SAMPLING_NUM) { + polyline_builder.addPoint(history.at(index).x, history.at(index).y); + } + if (i != 9) { + polyline_builder.addPoint(history.at(end).x, history.at(end).y); + } + polyline_builder.stroke("yellow", start / static_cast(history.size())) .strokeWidth(30); - visualizer->add(line_builder.getSvgString()); + visualizer->add(polyline_builder.getSvgString()); } } } for (const auto & history : enemy_history) { if (history.size() > SAMPLING_NUM + 1) { - for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) { - Point p1; - Point p2; - p1 << history.at(index).x, history.at(index).y; - p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y; - SvgLineBuilder line_builder; - line_builder.start(p1) - .end(p2) - .stroke("blue", index / static_cast(history.size())) + for (int i = 0; i < 10; i++) { + SvgPolyLineBuilder polyline_builder; + int start = static_cast((history.size() / 10.) * i); + int end = static_cast((history.size() / 10.) * (i + 1)); + for (int index = start; index < end; index += SAMPLING_NUM) { + polyline_builder.addPoint(history.at(index).x, history.at(index).y); + } + if (i != 9) { + polyline_builder.addPoint(history.at(end).x, history.at(end).y); + } + polyline_builder.stroke("blue", start / static_cast(history.size())) .strokeWidth(30); - visualizer->add(line_builder.getSvgString()); + visualizer->add(polyline_builder.getSvgString()); } } } if (ball_history.size() > SAMPLING_NUM + 1) { - for (int index = 0; index < ball_history.size() - SAMPLING_NUM; index += SAMPLING_NUM) { - 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())) + for (int i = 0; i < 10; i++) { + SvgPolyLineBuilder polyline_builder; + int start = static_cast((ball_history.size() / 10.) * i); + int end = static_cast((ball_history.size() / 10.) * (i + 1)); + for (int index = start; index < end; index += SAMPLING_NUM) { + polyline_builder.addPoint(ball_history.at(index)); + } + if (i != 9) { + polyline_builder.addPoint(ball_history.at(end)); + } + polyline_builder.stroke("orange", start / static_cast(ball_history.size())) .strokeWidth(30); - visualizer->add(line_builder.getSvgString()); + visualizer->add(polyline_builder.getSvgString()); } } visualizer->flush(); From 719632e45978121953632ac6b824af7d5928cb94 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 04:02:53 +0900 Subject: [PATCH 5/8] =?UTF-8?q?=E3=83=AD=E3=83=9C=E3=83=83=E3=83=88?= =?UTF-8?q?=E8=BB=8C=E9=81=93=E3=81=AE=E7=B7=9A=E3=82=92=E7=B4=B0=E3=81=8F?= =?UTF-8?q?=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index ee7a7c5ba..76c8fd468 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -453,7 +453,7 @@ void WorldModelPublisherComponent::publishWorldModel() polyline_builder.addPoint(history.at(end).x, history.at(end).y); } polyline_builder.stroke("yellow", start / static_cast(history.size())) - .strokeWidth(30); + .strokeWidth(15); visualizer->add(polyline_builder.getSvgString()); } } @@ -472,7 +472,7 @@ void WorldModelPublisherComponent::publishWorldModel() polyline_builder.addPoint(history.at(end).x, history.at(end).y); } polyline_builder.stroke("blue", start / static_cast(history.size())) - .strokeWidth(30); + .strokeWidth(15); visualizer->add(polyline_builder.getSvgString()); } } From b333f4c440e2f2904df5d3f6e77734501442317e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 04:59:00 +0900 Subject: [PATCH 6/8] =?UTF-8?q?Vision=E3=81=AB=E3=81=84=E3=81=AA=E3=81=84?= =?UTF-8?q?=E3=83=AD=E3=83=9C=E3=83=83=E3=83=88=E3=81=AE=E8=BB=8C=E9=81=93?= =?UTF-8?q?=E3=82=92=E8=A1=A8=E7=A4=BA=E3=81=97=E3=81=AA=E3=81=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/world_model_publisher.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 76c8fd468..3eea8b171 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -440,8 +440,10 @@ void WorldModelPublisherComponent::publishWorldModel() pub_world_model->publish(wm); constexpr int SAMPLING_NUM = 4; - for (const auto & history : friend_history) { - if (history.size() > SAMPLING_NUM + 1) { + for (const auto & [robot_id, history] : friend_history | ranges::views::enumerate) { + if ( + history.size() > SAMPLING_NUM + 1 && + robot_info[static_cast(our_color)].at(robot_id).detected) { for (int i = 0; i < 10; i++) { SvgPolyLineBuilder polyline_builder; int start = static_cast((history.size() / 10.) * i); @@ -459,8 +461,10 @@ void WorldModelPublisherComponent::publishWorldModel() } } - for (const auto & history : enemy_history) { - if (history.size() > SAMPLING_NUM + 1) { + for (const auto & [robot_id, history] : enemy_history | ranges::views::enumerate) { + if ( + history.size() > SAMPLING_NUM + 1 && + robot_info[static_cast(their_color)].at(robot_id).detected) { for (int i = 0; i < 10; i++) { SvgPolyLineBuilder polyline_builder; int start = static_cast((history.size() / 10.) * i); From ef36d464e1eef422fa12cb34a270e50ac975b35e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 05:00:03 +0900 Subject: [PATCH 7/8] =?UTF-8?q?SvgLayerArray=E3=82=92=E5=85=A8=E9=9D=A2?= =?UTF-8?q?=E7=9A=84=E3=81=AB=E6=8E=A1=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_visualizer_wrapper.hpp | 44 ++++++++++++++----- .../crane_visualization_aggregator_node.cpp | 11 ++--- 2 files changed, 39 insertions(+), 16 deletions(-) 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 index 345dd1e84..938a04610 100644 --- 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 @@ -8,8 +8,9 @@ #define CRANE_MSG_WRAPPERS__CRANE_VISUALIZER_WRAPPER_HPP_ #include -#include +#include #include +#include #include #include @@ -556,17 +557,17 @@ struct SvgPathBuilder struct CraneVisualizerBuffer { - using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray; + using SvgLayerArray = crane_visualization_interfaces::msg::SvgLayerArray; static inline std::unique_ptr buffer = nullptr; - rclcpp::Publisher::SharedPtr publisher; + rclcpp::Publisher::SharedPtr publisher; - SvgPrimitiveArray message_buffer; + SvgLayerArray message_buffer; template CraneVisualizerBuffer(Node & node, const std::string topic) { - publisher = node.template create_publisher(topic, rclcpp::SensorDataQoS()); + publisher = node.template create_publisher(topic, rclcpp::SensorDataQoS()); } template @@ -590,12 +591,26 @@ struct CraneVisualizerBuffer { if (active()) { buffer->publisher->publish(buffer->message_buffer); - buffer->message_buffer.svg_primitives.clear(); + buffer->message_buffer.svg_primitive_arrays.clear(); + } + } + + static auto clear(std::string layer = "") -> void + { + if (CraneVisualizerBuffer::active()) { + if (layer == "") { + CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays.clear(); + } else { + ranges::actions::remove_if( + CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays, + [&layer](const auto & layer_array) { return layer_array.layer == layer; }); + } } } struct MessageBuilder { + using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray; using SharedPtr = std::shared_ptr; using UniquePtr = std::unique_ptr; @@ -606,15 +621,22 @@ struct CraneVisualizerBuffer 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()); + SvgPrimitiveArray layer_msg; + layer_msg.layer = layer; + layer_msg.svg_primitives = message_buffer; + CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays.push_back(layer_msg); message_buffer.clear(); } } - using SvgPrimitiveArray = crane_visualization_interfaces::msg::SvgPrimitiveArray; + void clear() { message_buffer.clear(); } + + void clearBuffer() + { + if (CraneVisualizerBuffer::active()) { + CraneVisualizerBuffer::clear(layer); + } + } std::vector message_buffer; diff --git a/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp index c79777ecc..81c2ffe25 100644 --- a/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp +++ b/utility/crane_visualization_aggregator/src/crane_visualization_aggregator_node.cpp @@ -21,11 +21,13 @@ class VisualizationAggregator : public rclcpp::Node public: VisualizationAggregator() : Node("visualization_aggregator") { - subscriber = create_subscription( + subscriber = create_subscription( "/visualizer_svgs", rclcpp::SensorDataQoS(), - [&](const crane_visualization_interfaces::msg::SvgPrimitiveArray::ConstSharedPtr & msg) { + [&](const crane_visualization_interfaces::msg::SvgLayerArray::ConstSharedPtr & msg) { // store into　layers - layers[msg->layer] = msg->svg_primitives; + for (const auto & layer_msg : msg->svg_primitive_arrays) { + layers[layer_msg.layer] = layer_msg.svg_primitives; + } }); publisher = create_publisher("/aggregated_svgs", 10); @@ -43,8 +45,7 @@ class VisualizationAggregator : public rclcpp::Node } private: - rclcpp::Subscription::SharedPtr - subscriber; + rclcpp::Subscription::SharedPtr subscriber; rclcpp::Publisher::SharedPtr publisher; std::unordered_map> layers; From fa8f453ad0866c322d36c7c8f1529307dd191618 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 1 Feb 2025 06:08:49 +0900 Subject: [PATCH 8/8] =?UTF-8?q?=E3=83=90=E3=83=83=E3=83=95=E3=82=A1?= =?UTF-8?q?=E3=82=AF=E3=83=AA=E3=82=A2?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/skill_base.hpp | 2 ++ .../include/crane_planner_plugins/planner_base.hpp | 2 ++ .../crane_msg_wrappers/crane_visualizer_wrapper.hpp | 10 +++++++--- 3 files changed, 11 insertions(+), 3 deletions(-) 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 fffe99c5f..98cc8ed03 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -149,6 +149,8 @@ class SkillInterface command_base->latest_msg.skill_name = name; } + virtual ~SkillInterface() { visualizer->clearBuffer(); } + const std::string name; virtual Status run( 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 d3516fe91..f43bb02f3 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 @@ -54,6 +54,8 @@ class PlannerBase { } + virtual ~PlannerBase() { visualizer->clearBuffer(); } + crane_msgs::srv::RobotSelect::Response doRobotSelect( const crane_msgs::srv::RobotSelect::Request::SharedPtr request, const std::unordered_map & prev_roles, PlannerContext & context) 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 index 938a04610..4418426f2 100644 --- 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 @@ -601,9 +601,12 @@ struct CraneVisualizerBuffer if (layer == "") { CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays.clear(); } else { - ranges::actions::remove_if( - CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays, - [&layer](const auto & layer_array) { return layer_array.layer == layer; }); + for (auto & svg_layer : CraneVisualizerBuffer::buffer->message_buffer.svg_primitive_arrays | + ranges::views::filter([&](auto svg_primitive_array) { + return svg_primitive_array.layer == layer; + })) { + svg_layer.svg_primitives.clear(); + } } } } @@ -633,6 +636,7 @@ struct CraneVisualizerBuffer void clearBuffer() { + clear(); if (CraneVisualizerBuffer::active()) { CraneVisualizerBuffer::clear(layer); }