ロボット軌跡可視化 (#657)

crane_world_model_publisher/CMakeLists.txt

@@ -26,7 +26,7 @@ target_link_libraries(${PROJECT_NAME}_component
 rclcpp_components_register_nodes(${PROJECT_NAME}_component "crane::WorldModelPublisherComponent")
 
 ament_auto_add_executable(${PROJECT_NAME}_node src/world_model_publisher_node.cpp)
-target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_component)
+target_link_libraries(${PROJECT_NAME}_node ${PROJECT_NAME}_component glog)
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)

crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp

@@ -50,13 +50,16 @@ extern "C" {
 #include <robocup_ssl_msgs/ssl_vision_geometry.pb.h>
 #include <robocup_ssl_msgs/ssl_vision_wrapper.pb.h>
 
+#include <crane_basics/boost_geometry.hpp>
+#include <crane_msg_wrappers/consai_visualizer_wrapper.hpp>
 #include <crane_msgs/msg/ball_info.hpp>
 #include <crane_msgs/msg/play_situation.hpp>
 #include <crane_msgs/msg/robot_feedback_array.hpp>
 #include <crane_msgs/msg/robot_info.hpp>
 #include <crane_msgs/msg/world_model.hpp>
 #include <crane_world_model_publisher/multicast.hpp>
 #include <crane_world_model_publisher/visualization_data_handler.hpp>
+#include <deque>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <robocup_ssl_msgs/msg/referee.hpp>
@@ -165,6 +168,13 @@ class WorldModelPublisherComponent : public rclcpp::Node
 
   bool ball_event_detected = false;
 
+  ConsaiVisualizerBuffer::MessageBuilder::UniquePtr visualizer;
+
+  std::array<std::deque<geometry_msgs::msg::Pose2D>, 20> friend_history;
+  std::array<std::deque<geometry_msgs::msg::Pose2D>, 20> enemy_history;
+
+  int history_size;
+
   enum class BallEvent {
     NONE,
     OUR_BALL,

crane_world_model_publisher/package.xml

@@ -12,7 +12,9 @@
   <depend>boost</depend>
   <depend>consai_visualizer_msgs</depend>
   <depend>crane_basics</depend>
+  <depend>crane_msg_wrappers</depend>
   <depend>crane_msgs</depend>
+  <depend>libgoogle-glog-dev</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>robocup_ssl_msgs</depend>

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -13,7 +13,10 @@
 namespace crane
 {
 WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOptions & options)
-: rclcpp::Node("world_model_publisher", options), vis_data_handler(*this)
+: rclcpp::Node("world_model_publisher", options),
+  vis_data_handler(*this),
+  visualizer(
+    std::make_unique<ConsaiVisualizerBuffer::MessageBuilder>("world_model_publisher", "trajectory"))
 {
   using std::chrono_literals::operator""ms;
   declare_parameter("tracker_address", "224.5.23.2");
@@ -26,6 +29,12 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
   geometry_receiver = std::make_unique<multicast::MulticastReceiver>(
     get_parameter("vision_address").get_value<std::string>(),
     get_parameter("vision_port").get_value<int>());
+
+  crane::ConsaiVisualizerBuffer::activate(*this);
+
+  declare_parameter("position_history_size", 100);
+  get_parameter<int>("position_history_size", history_size);
+
   udp_timer = rclcpp::create_timer(
     this, get_clock(), 10ms, std::bind(&WorldModelPublisherComponent::on_udp_timer, this));
 
@@ -100,13 +109,6 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
 
   pub_world_model = create_publisher<crane_msgs::msg::WorldModel>("/world_model", 1);
 
-  using std::chrono::operator""ms;
-  timer = rclcpp::create_timer(this, get_clock(), 16ms, [this]() {
-    if (has_vision_updated && has_geometry_updated) {
-      publishWorldModel();
-    }
-  });
-
   declare_parameter("team_name", "ibis-ssl");
   team_name = get_parameter("team_name").as_string();
 
@@ -153,6 +155,13 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
         ball_placement_target_y = msg.designated_position.front().y / 1000.;
       }
     });
+
+  using std::chrono::operator""ms;
+  timer = rclcpp::create_timer(this, get_clock(), 16ms, [this]() {
+    if (has_vision_updated && has_geometry_updated) {
+      publishWorldModel();
+    }
+  });
 }
 
 void WorldModelPublisherComponent::on_udp_timer()
@@ -343,6 +352,12 @@ void WorldModelPublisherComponent::publishWorldModel()
     info.velocity = robot.velocity;
     info.ball_contact = robot.ball_contact;
     wm.robot_info_ours.emplace_back(info);
+    if (robot.detected) {
+      friend_history[robot.robot_id].push_back(robot.pose);
+    }
+    if (friend_history[robot.robot_id].size() > history_size) {
+      friend_history[robot.robot_id].pop_front();
+    }
   }
   for (const auto & robot : robot_info[static_cast<uint8_t>(their_color)]) {
     crane_msgs::msg::RobotInfoTheirs info;
@@ -353,6 +368,12 @@ void WorldModelPublisherComponent::publishWorldModel()
     info.velocity = robot.velocity;
     info.ball_contact = robot.ball_contact;
     wm.robot_info_theirs.emplace_back(info);
+    if (robot.detected) {
+      enemy_history[robot.robot_id].push_back(robot.pose);
+    }
+    if (enemy_history[robot.robot_id].size() > history_size) {
+      enemy_history[robot.robot_id].pop_front();
+    }
   }
 
   wm.field_info.x = field_w;
@@ -372,6 +393,22 @@ void WorldModelPublisherComponent::publishWorldModel()
   wm.header.stamp = rclcpp::Clock().now();
 
   pub_world_model->publish(wm);
+
+  for (const auto & history : friend_history) {
+    for (int index = 0; const auto & pose : history) {
+      visualizer->addPoint(
+        pose.x, pose.y, 2, "yellow", index++ / static_cast<double>(history.size()));
+    }
+  }
+
+  for (const auto & history : enemy_history) {
+    for (int index = 0; const auto & pose : history) {
+      visualizer->addPoint(
+        pose.x, pose.y, 2, "blue", index++ / static_cast<double>(history.size()));
+    }
+  }
+  visualizer->flush();
+  ConsaiVisualizerBuffer::publish();
 }
 
 void WorldModelPublisherComponent::updateBallContact()

crane_world_model_publisher/src/world_model_publisher_node.cpp

@@ -4,12 +4,17 @@
 // license that can be found in the LICENSE file or at
 // https://opensource.org/licenses/MIT.
 
+#include <glog/logging.h>
+
 #include <rclcpp/rclcpp.hpp>
 
 #include "crane_world_model_publisher/world_model_publisher.hpp"
 
 int main(int argc, char * argv[])
 {
+  google::InitGoogleLogging(argv[0]);
+  google::InstallFailureSignalHandler();
+
   rclcpp::init(argc, argv);
   auto node = std::make_shared<crane::WorldModelPublisherComponent>(rclcpp::NodeOptions());
   rclcpp::spin(node);