ament_clang_format

.clang-format

@@ -9,7 +9,7 @@ BraceWrapping:
   AfterNamespace: true
   AfterStruct: true
 BreakBeforeBraces: Custom
-ColumnLimit: 80
+ColumnLimit: 100
 ConstructorInitializerIndentWidth: 0
 ContinuationIndentWidth: 2
 DerivePointerAlignment: false

consai_ros2/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp

@@ -34,11 +34,9 @@ using TrackedBall = robocup_ssl_msgs::msg::TrackedBall;
 
 using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian;
 
-using SystemModelGaussianUncertainty =
-  BFL::LinearAnalyticSystemModelGaussianUncertainty;
+using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty;
 
-using MeasurementModelGaussianUncertainty =
-  BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
+using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
 
 using Gaussian = BFL::Gaussian;
 

consai_ros2/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp

@@ -34,11 +34,9 @@ using TrackedRobot = robocup_ssl_msgs::msg::TrackedRobot;
 
 using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian;
 
-using SystemModelGaussianUncertainty =
-  BFL::LinearAnalyticSystemModelGaussianUncertainty;
+using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty;
 
-using MeasurementModelGaussianUncertainty =
-  BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
+using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
 
 using Gaussian = BFL::Gaussian;
 

consai_ros2/consai_vision_tracker/include/consai_vision_tracker/visualization_data_handler.hpp

@@ -32,8 +32,7 @@ using GeometryData = robocup_ssl_msgs::msg::GeometryData;
 class VisualizationDataHandler
 {
 public:
-  explicit VisualizationDataHandler(
-    const rclcpp::Publisher<VisualizerObjects>::SharedPtr ptr);
+  explicit VisualizationDataHandler(const rclcpp::Publisher<VisualizerObjects>::SharedPtr ptr);
   ~VisualizationDataHandler() = default;
 
   void publish_vis_detection(const DetectionFrame::SharedPtr msg);

consai_ros2/consai_vision_tracker/src/ball_tracker.cpp

@@ -84,11 +84,9 @@ BallTracker::BallTracker(const double dt)
 
   // 位置、速度の変化をのシステムノイズで表現する（つまりめちゃくちゃノイズがでかい）
   // 0 m/s から、いきなり1.0 m/sに変化しうる、ということ
-  const double MAX_LINEAR_ACC_MPS =
-    1.0 / dt;  // 例：1.0[m/s] / 0.001[s] = 100 [m/ss]
-  const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt;  // [m/s]
-  const double MAX_LINEAR_MOVEMENT_IN_DT =
-    MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2);  // [m]
+  const double MAX_LINEAR_ACC_MPS = 1.0 / dt;  // 例：1.0[m/s] / 0.001[s] = 100 [m/ss]
+  const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt;                      // [m/s]
+  const double MAX_LINEAR_MOVEMENT_IN_DT = MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2);  // [m]
 
   // システムノイズの分散
   SymmetricMatrix sys_noise_cov(4);
@@ -121,8 +119,7 @@ BallTracker::BallTracker(const double dt)
   Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov);
 
   meas_pdf = std::make_shared<ConditionalGaussian>(H, measurement_uncertainty);
-  meas_model =
-    std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
+  meas_model = std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
 
   // 事前分布
   ColumnVector prior_mu(4);
@@ -252,8 +249,8 @@ bool BallTracker::is_outlier(const TrackedBall & observation) const
     return false;
   }
 
-  double mahalanobis = std::sqrt(
-    std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
+  double mahalanobis =
+    std::sqrt(std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
   if (mahalanobis > THRESHOLD) {
     return true;
   }

consai_ros2/consai_vision_tracker/src/robot_tracker.cpp

@@ -123,12 +123,10 @@ RobotTracker::RobotTracker(const int team_color, const int id, const double dt)
   // 例：1.0[rad/s] / 0.001[s] = 100 [rad/ss]
   // cspell: ignore RADPS
   const double MAX_ANGULAR_ACC_RADPS = 0.05 * M_PI / dt;
-  const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt;      // [m/s]
-  const double MAX_ANGULAR_ACCEL_IN_DT = MAX_ANGULAR_ACC_RADPS * dt;  // [rad/s]
-  const double MAX_LINEAR_MOVEMENT_IN_DT =
-    MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2);  // [m]
-  const double MAX_ANGULAR_MOVEMENT_IN_DT =
-    MAX_ANGULAR_ACC_RADPS / 2 * std::pow(dt, 2);  // [rad]
+  const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt;                          // [m/s]
+  const double MAX_ANGULAR_ACCEL_IN_DT = MAX_ANGULAR_ACC_RADPS * dt;                      // [rad/s]
+  const double MAX_LINEAR_MOVEMENT_IN_DT = MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2);      // [m]
+  const double MAX_ANGULAR_MOVEMENT_IN_DT = MAX_ANGULAR_ACC_RADPS / 2 * std::pow(dt, 2);  // [rad]
 
   // システムノイズの分散
   SymmetricMatrix sys_noise_cov(6);
@@ -165,8 +163,7 @@ RobotTracker::RobotTracker(const int team_color, const int id, const double dt)
   Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov);
 
   meas_pdf = std::make_shared<ConditionalGaussian>(H, measurement_uncertainty);
-  meas_model =
-    std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
+  meas_model = std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
 
   // 事前分布
   ColumnVector prior_mu(6);
@@ -239,8 +236,7 @@ TrackedRobot RobotTracker::update()
     double sum_x = 0.0;
     double sum_y = 0.0;
 
-    for (auto it = robot_observations.begin();
-         it != robot_observations.end();) {
+    for (auto it = robot_observations.begin(); it != robot_observations.end();) {
       mean_observation(1) += it->pos.x;
       mean_observation(2) += it->pos.y;
       // 角度は-pi ~ piの範囲なので、2次元ベクトルに変換してから平均値を求める
@@ -256,8 +252,8 @@ TrackedRobot RobotTracker::update()
 
     // 観測値と前回の予測値がpi, -pi付近にあるとき、
     // ２つの角度の差分が大きくならないように、観測値の符号と値を調節する
-    mean_observation(3) = normalize_orientation(
-      filter->PostGet()->ExpectedValueGet()(3), mean_observation(3));
+    mean_observation(3) =
+      normalize_orientation(filter->PostGet()->ExpectedValueGet()(3), mean_observation(3));
 
     filter->Update(meas_model.get(), mean_observation);
     correct_orientation_overflow_of_prior();
@@ -324,8 +320,8 @@ bool RobotTracker::is_outlier(const TrackedRobot & observation) const
     return false;
   }
 
-  double mahalanobis = std::sqrt(
-    std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
+  double mahalanobis =
+    std::sqrt(std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
   if (mahalanobis > THRESHOLD) {
     return true;
   }
@@ -361,8 +357,7 @@ double RobotTracker::normalize_orientation(double orientation) const
   return orientation;
 }
 
-double RobotTracker::normalize_orientation(
-  const double from, const double to) const
+double RobotTracker::normalize_orientation(const double from, const double to) const
 {
   // fromからtoへ連続に角度が変化するようにtoの符号と大きさを変更する
   // from(150 deg) -> to(-150 deg) => from(150 deg) -> to(210 deg)

consai_ros2/consai_vision_tracker/src/tracker_component.cpp

@@ -35,26 +35,24 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) : Node("tracker", options)
 
   ball_tracker = std::make_shared<BallTracker>(UPDATE_RATE.count());
   for (int i = 0; i < 16; i++) {
-    blue_robot_tracker.push_back(std::make_shared<RobotTracker>(
-      RobotId::TEAM_COLOR_BLUE, i, UPDATE_RATE.count()));
-    yellow_robot_tracker.push_back(std::make_shared<RobotTracker>(
-      RobotId::TEAM_COLOR_YELLOW, i, UPDATE_RATE.count()));
+    blue_robot_tracker.push_back(
+      std::make_shared<RobotTracker>(RobotId::TEAM_COLOR_BLUE, i, UPDATE_RATE.count()));
+    yellow_robot_tracker.push_back(
+      std::make_shared<RobotTracker>(RobotId::TEAM_COLOR_YELLOW, i, UPDATE_RATE.count()));
   }
 
   timer = create_wall_timer(UPDATE_RATE, std::bind(&Tracker::on_timer, this));
 
   pub_tracked = create_publisher<TrackedFrame>("detection_tracked", 10);
 
   vis_data_handler_ = std::make_shared<VisualizationDataHandler>(
-    create_publisher<VisualizerObjects>(
-      "visualizer_objects", rclcpp::SensorDataQoS()));
+    create_publisher<VisualizerObjects>("visualizer_objects", rclcpp::SensorDataQoS()));
 
   declare_parameter("invert", false);
 
   if (get_parameter("invert").get_value<bool>()) {
     sub_detection = create_subscription<DetectionFrame>(
-      "detection", 10,
-      std::bind(&Tracker::callback_detection_invert, this, _1));
+      "detection", 10, std::bind(&Tracker::callback_detection_invert, this, _1));
   } else {
     sub_detection = create_subscription<DetectionFrame>(
       "detection", 10, std::bind(&Tracker::callback_detection, this, _1));
@@ -91,15 +89,13 @@ void Tracker::callback_detection(const DetectionFrame::SharedPtr msg)
 
   for (const auto & blue_robot : msg->robots_blue) {
     if (blue_robot.robot_id.size() > 0) {
-      blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(
-        blue_robot);
+      blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(blue_robot);
     }
   }
 
   for (const auto & yellow_robot : msg->robots_yellow) {
     if (yellow_robot.robot_id.size() > 0) {
-      yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(
-        yellow_robot);
+      yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot);
     }
   }
 
@@ -118,16 +114,14 @@ void Tracker::callback_detection_invert(const DetectionFrame::SharedPtr msg)
   for (auto && blue_robot : msg->robots_blue) {
     if (blue_robot.robot_id.size() > 0) {
       invert_robot(blue_robot);
-      blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(
-        blue_robot);
+      blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(blue_robot);
     }
   }
 
   for (auto && yellow_robot : msg->robots_yellow) {
     if (yellow_robot.robot_id.size() > 0) {
       invert_robot(yellow_robot);
-      yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(
-        yellow_robot);
+      yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot);
     }
   }
 

consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp

@@ -40,8 +40,7 @@ VisualizationDataHandler::VisualizationDataHandler(
 {
 }
 
-void VisualizationDataHandler::publish_vis_detection(
-  const DetectionFrame::SharedPtr msg)
+void VisualizationDataHandler::publish_vis_detection(const DetectionFrame::SharedPtr msg)
 {
   // detectionを描画情報に変換してpublishする
   auto vis_objects = std::make_unique<VisualizerObjects>();
@@ -108,8 +107,7 @@ void VisualizationDataHandler::publish_vis_detection(
   }
 }
 
-void VisualizationDataHandler::publish_vis_geometry(
-  const GeometryData::SharedPtr msg)
+void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPtr msg)
 {
   // geometryを描画情報に変換してpublishする
   auto vis_objects = std::make_unique<VisualizerObjects>();
@@ -170,18 +168,15 @@ void VisualizationDataHandler::publish_vis_geometry(
   rect.line_size = 3;
   rect.center.x = 0.0;
   rect.center.y = 0.0;
-  rect.width =
-    (msg->field.field_length + msg->field.boundary_width * 2) * 0.001;
-  rect.height =
-    (msg->field.field_width + msg->field.boundary_width * 2) * 0.001;
+  rect.width = (msg->field.field_length + msg->field.boundary_width * 2) * 0.001;
+  rect.height = (msg->field.field_width + msg->field.boundary_width * 2) * 0.001;
   rect.caption = "wall";
   vis_objects->rects.push_back(rect);
 
   pub_vis_objects_->publish(std::move(vis_objects));
 }
 
-TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(
-  TrackedFrame::UniquePtr msg)
+TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(TrackedFrame::UniquePtr msg)
 {
   const double VELOCITY_ALPHA = 0.5;
   // tracked_frameを描画情報に変換してpublishする

consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/game_controller_component.hpp

@@ -37,8 +37,7 @@ class GameController : public rclcpp::Node
   void on_timer();
 
 private:
-  robocup_ssl_msgs::msg::TeamInfo parse_team_info(
-    const Referee_TeamInfo & team_info);
+  robocup_ssl_msgs::msg::TeamInfo parse_team_info(const Referee_TeamInfo & team_info);
 
   rclcpp::TimerBase::SharedPtr timer;
 

consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/grsim_component.hpp

@@ -50,13 +50,10 @@ class GrSim : public rclcpp::Node
 
   void callback_replacement(const Replacement::SharedPtr msg);
 
-  void set_command(
-    grSim_Robot_Command * robot_command,
-    const RobotCommand & msg_robot_command);
+  void set_command(grSim_Robot_Command * robot_command, const RobotCommand & msg_robot_command);
 
   void set_robot_replacement(
-    grSim_RobotReplacement * robot_replacement,
-    const RobotReplacement & msg_robot_replacement);
+    grSim_RobotReplacement * robot_replacement, const RobotReplacement & msg_robot_replacement);
 
   std::unique_ptr<udp_sender::UDPSender> sender;
 

consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/udp_sender.hpp

@@ -37,10 +37,7 @@ class UDPSender
     endpoint = *resolver.resolve(query);
   }
 
-  void send(const std::string & str)
-  {
-    socket.send_to(asio::buffer(str), endpoint);
-  }
+  void send(const std::string & str) { socket.send_to(asio::buffer(str), endpoint); }
 
 private:
   asio::io_service io_service;

consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/vision_component.hpp

@@ -44,8 +44,7 @@ class Vision : public rclcpp::Node
   void publish_geometry(const SSL_GeometryData & geometry_data);
 
   void set_geometry_field_size(
-    robocup_ssl_msgs::msg::GeometryFieldSize & msg_field,
-    const SSL_GeometryFieldSize & data_field);
+    robocup_ssl_msgs::msg::GeometryFieldSize & msg_field, const SSL_GeometryFieldSize & data_field);
 
   robocup_ssl_msgs::msg::GeometryCameraCalibration parse_calib(
     const SSL_GeometryCameraCalibration & data_calib);
@@ -54,11 +53,9 @@ class Vision : public rclcpp::Node
 
   std::unique_ptr<multicast::MulticastReceiver> receiver;
 
-  rclcpp::Publisher<robocup_ssl_msgs::msg::DetectionFrame>::SharedPtr
-    pub_detection;
+  rclcpp::Publisher<robocup_ssl_msgs::msg::DetectionFrame>::SharedPtr pub_detection;
 
-  rclcpp::Publisher<robocup_ssl_msgs::msg::GeometryData>::SharedPtr
-    pub_geometry;
+  rclcpp::Publisher<robocup_ssl_msgs::msg::GeometryData>::SharedPtr pub_geometry;
 };
 
 }  // namespace robocup_ssl_comm

consai_ros2/robocup_ssl_comm/src/game_controller_component.cpp

@@ -69,8 +69,7 @@ void GameController::on_timer()
         referee_msg->designated_position.push_back(point);
       }
       if (packet.has_blue_team_on_positive_half()) {
-        referee_msg->blue_team_on_positive_half.push_back(
-          packet.blue_team_on_positive_half());
+        referee_msg->blue_team_on_positive_half.push_back(packet.blue_team_on_positive_half());
       }
       if (packet.has_next_command()) {
         referee_msg->next_command.push_back(packet.next_command());
@@ -85,8 +84,7 @@ void GameController::on_timer()
   }
 }
 
-robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(
-  const Referee_TeamInfo & team_info)
+robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(const Referee_TeamInfo & team_info)
 {
   robocup_ssl_msgs::msg::TeamInfo parsed_team_info;
 
@@ -104,8 +102,7 @@ robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(
     parsed_team_info.foul_counter.push_back(team_info.foul_counter());
   }
   if (team_info.has_ball_placement_failures()) {
-    parsed_team_info.ball_placement_failures.push_back(
-      team_info.ball_placement_failures());
+    parsed_team_info.ball_placement_failures.push_back(team_info.ball_placement_failures());
   }
   if (team_info.has_can_place_ball()) {
     parsed_team_info.can_place_ball.push_back(team_info.can_place_ball());
@@ -114,8 +111,7 @@ robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(
     parsed_team_info.max_allowed_bots.push_back(team_info.max_allowed_bots());
   }
   if (team_info.has_bot_substitution_intent()) {
-    parsed_team_info.bot_substitution_intent.push_back(
-      team_info.bot_substitution_intent());
+    parsed_team_info.bot_substitution_intent.push_back(team_info.bot_substitution_intent());
   }
   if (team_info.has_ball_placement_failures_reached()) {
     parsed_team_info.ball_placement_failures_reached.push_back(

consai_ros2/robocup_ssl_comm/src/grsim_component.cpp

@@ -31,8 +31,8 @@ GrSim::GrSim(const rclcpp::NodeOptions & options) : Node("grsim", options)
 {
   sender = std::make_unique<udp_sender::UDPSender>("127.0.0.1", 20011);
 
-  sub_commands = create_subscription<Commands>(
-    "commands", 10, std::bind(&GrSim::callback_commands, this, _1));
+  sub_commands =
+    create_subscription<Commands>("commands", 10, std::bind(&GrSim::callback_commands, this, _1));
   sub_replacement = create_subscription<Replacement>(
     "replacement", 10, std::bind(&GrSim::callback_replacement, this, _1));
 }
@@ -89,8 +89,7 @@ void GrSim::callback_replacement(const Replacement::SharedPtr msg)
   sender->send(output);
 }
 
-void GrSim::set_command(
-  grSim_Robot_Command * robot_command, const RobotCommand & msg_robot_command)
+void GrSim::set_command(grSim_Robot_Command * robot_command, const RobotCommand & msg_robot_command)
 {
   robot_command->set_id(msg_robot_command.id);
   robot_command->set_kickspeedx(msg_robot_command.kickspeedx);
@@ -115,8 +114,7 @@ void GrSim::set_command(
 }
 
 void GrSim::set_robot_replacement(
-  grSim_RobotReplacement * robot_replacement,
-  const RobotReplacement & msg_robot_replacement)
+  grSim_RobotReplacement * robot_replacement, const RobotReplacement & msg_robot_replacement)
 {
   robot_replacement->set_x(msg_robot_replacement.x);
   robot_replacement->set_y(msg_robot_replacement.y);