Skip to content

Commit c910ea9

Browse files
HansRoboHansRobo
committed
ament_clang_format
1 parent 2a8b9cf commit c910ea9

File tree

56 files changed

+512
-976
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

56 files changed

+512
-976
lines changed

‎.clang-format

+1-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ BraceWrapping:
99
AfterNamespace: true
1010
AfterStruct: true
1111
BreakBeforeBraces: Custom
12-
ColumnLimit: 80
12+
ColumnLimit: 100
1313
ConstructorInitializerIndentWidth: 0
1414
ContinuationIndentWidth: 2
1515
DerivePointerAlignment: false

‎consai_ros2/consai_vision_tracker/include/consai_vision_tracker/ball_tracker.hpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,9 @@ using TrackedBall = robocup_ssl_msgs::msg::TrackedBall;
3434

3535
using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian;
3636

37-
using SystemModelGaussianUncertainty =
38-
BFL::LinearAnalyticSystemModelGaussianUncertainty;
37+
using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty;
3938

40-
using MeasurementModelGaussianUncertainty =
41-
BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
39+
using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
4240

4341
using Gaussian = BFL::Gaussian;
4442

‎consai_ros2/consai_vision_tracker/include/consai_vision_tracker/robot_tracker.hpp

+2-4
Original file line numberDiff line numberDiff line change
@@ -34,11 +34,9 @@ using TrackedRobot = robocup_ssl_msgs::msg::TrackedRobot;
3434

3535
using ConditionalGaussian = BFL::LinearAnalyticConditionalGaussian;
3636

37-
using SystemModelGaussianUncertainty =
38-
BFL::LinearAnalyticSystemModelGaussianUncertainty;
37+
using SystemModelGaussianUncertainty = BFL::LinearAnalyticSystemModelGaussianUncertainty;
3938

40-
using MeasurementModelGaussianUncertainty =
41-
BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
39+
using MeasurementModelGaussianUncertainty = BFL::LinearAnalyticMeasurementModelGaussianUncertainty;
4240

4341
using Gaussian = BFL::Gaussian;
4442

‎consai_ros2/consai_vision_tracker/include/consai_vision_tracker/visualization_data_handler.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,7 @@ using GeometryData = robocup_ssl_msgs::msg::GeometryData;
3232
class VisualizationDataHandler
3333
{
3434
public:
35-
explicit VisualizationDataHandler(
36-
const rclcpp::Publisher<VisualizerObjects>::SharedPtr ptr);
35+
explicit VisualizationDataHandler(const rclcpp::Publisher<VisualizerObjects>::SharedPtr ptr);
3736
~VisualizationDataHandler() = default;
3837

3938
void publish_vis_detection(const DetectionFrame::SharedPtr msg);

‎consai_ros2/consai_vision_tracker/src/ball_tracker.cpp

+6-9
Original file line numberDiff line numberDiff line change
@@ -84,11 +84,9 @@ BallTracker::BallTracker(const double dt)
8484

8585
// 位置、速度の変化をのシステムノイズで表現する（つまりめちゃくちゃノイズがでかい）
8686
// 0 m/s から、いきなり1.0 m/sに変化しうる、ということ
87-
const double MAX_LINEAR_ACC_MPS =
88-
1.0 / dt; // 例：1.0[m/s] / 0.001[s] = 100 [m/ss]
89-
const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt; // [m/s]
90-
const double MAX_LINEAR_MOVEMENT_IN_DT =
91-
MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2); // [m]
87+
const double MAX_LINEAR_ACC_MPS = 1.0 / dt; // 例：1.0[m/s] / 0.001[s] = 100 [m/ss]
88+
const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt; // [m/s]
89+
const double MAX_LINEAR_MOVEMENT_IN_DT = MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2); // [m]
9290

9391
// システムノイズの分散
9492
SymmetricMatrix sys_noise_cov(4);
@@ -121,8 +119,7 @@ BallTracker::BallTracker(const double dt)
121119
Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov);
122120

123121
meas_pdf = std::make_shared<ConditionalGaussian>(H, measurement_uncertainty);
124-
meas_model =
125-
std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
122+
meas_model = std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
126123

127124
// 事前分布
128125
ColumnVector prior_mu(4);
@@ -252,8 +249,8 @@ bool BallTracker::is_outlier(const TrackedBall & observation) const
252249
return false;
253250
}
254251

255-
double mahalanobis = std::sqrt(
256-
std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
252+
double mahalanobis =
253+
std::sqrt(std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
257254
if (mahalanobis > THRESHOLD) {
258255
return true;
259256
}

‎consai_ros2/consai_vision_tracker/src/robot_tracker.cpp

+11-16
Original file line numberDiff line numberDiff line change
@@ -123,12 +123,10 @@ RobotTracker::RobotTracker(const int team_color, const int id, const double dt)
123123
// 例：1.0[rad/s] / 0.001[s] = 100 [rad/ss]
124124
// cspell: ignore RADPS
125125
const double MAX_ANGULAR_ACC_RADPS = 0.05 * M_PI / dt;
126-
const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt; // [m/s]
127-
const double MAX_ANGULAR_ACCEL_IN_DT = MAX_ANGULAR_ACC_RADPS * dt; // [rad/s]
128-
const double MAX_LINEAR_MOVEMENT_IN_DT =
129-
MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2); // [m]
130-
const double MAX_ANGULAR_MOVEMENT_IN_DT =
131-
MAX_ANGULAR_ACC_RADPS / 2 * std::pow(dt, 2); // [rad]
126+
const double MAX_LINEAR_ACCEL_IN_DT = MAX_LINEAR_ACC_MPS * dt; // [m/s]
127+
const double MAX_ANGULAR_ACCEL_IN_DT = MAX_ANGULAR_ACC_RADPS * dt; // [rad/s]
128+
const double MAX_LINEAR_MOVEMENT_IN_DT = MAX_LINEAR_ACC_MPS / 2 * std::pow(dt, 2); // [m]
129+
const double MAX_ANGULAR_MOVEMENT_IN_DT = MAX_ANGULAR_ACC_RADPS / 2 * std::pow(dt, 2); // [rad]
132130

133131
// システムノイズの分散
134132
SymmetricMatrix sys_noise_cov(6);
@@ -165,8 +163,7 @@ RobotTracker::RobotTracker(const int team_color, const int id, const double dt)
165163
Gaussian measurement_uncertainty(meas_noise_mu, meas_noise_cov);
166164

167165
meas_pdf = std::make_shared<ConditionalGaussian>(H, measurement_uncertainty);
168-
meas_model =
169-
std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
166+
meas_model = std::make_shared<MeasurementModelGaussianUncertainty>(meas_pdf.get());
170167

171168
// 事前分布
172169
ColumnVector prior_mu(6);
@@ -239,8 +236,7 @@ TrackedRobot RobotTracker::update()
239236
double sum_x = 0.0;
240237
double sum_y = 0.0;
241238

242-
for (auto it = robot_observations.begin();
243-
it != robot_observations.end();) {
239+
for (auto it = robot_observations.begin(); it != robot_observations.end();) {
244240
mean_observation(1) += it->pos.x;
245241
mean_observation(2) += it->pos.y;
246242
// 角度は-pi ~ piの範囲なので、2次元ベクトルに変換してから平均値を求める
@@ -256,8 +252,8 @@ TrackedRobot RobotTracker::update()
256252

257253
// 観測値と前回の予測値がpi, -pi付近にあるとき、
258254
// ２つの角度の差分が大きくならないように、観測値の符号と値を調節する
259-
mean_observation(3) = normalize_orientation(
260-
filter->PostGet()->ExpectedValueGet()(3), mean_observation(3));
255+
mean_observation(3) =
256+
normalize_orientation(filter->PostGet()->ExpectedValueGet()(3), mean_observation(3));
261257

262258
filter->Update(meas_model.get(), mean_observation);
263259
correct_orientation_overflow_of_prior();
@@ -324,8 +320,8 @@ bool RobotTracker::is_outlier(const TrackedRobot & observation) const
324320
return false;
325321
}
326322

327-
double mahalanobis = std::sqrt(
328-
std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
323+
double mahalanobis =
324+
std::sqrt(std::pow(diff_x, 2) / covariance_x + std::pow(diff_y, 2) / covariance_y);
329325
if (mahalanobis > THRESHOLD) {
330326
return true;
331327
}
@@ -361,8 +357,7 @@ double RobotTracker::normalize_orientation(double orientation) const
361357
return orientation;
362358
}
363359

364-
double RobotTracker::normalize_orientation(
365-
const double from, const double to) const
360+
double RobotTracker::normalize_orientation(const double from, const double to) const
366361
{
367362
// fromからtoへ連続に角度が変化するようにtoの符号と大きさを変更する
368363
// from(150 deg) -> to(-150 deg) => from(150 deg) -> to(210 deg)

‎consai_ros2/consai_vision_tracker/src/tracker_component.cpp

+10-16
Original file line numberDiff line numberDiff line change
@@ -35,26 +35,24 @@ Tracker::Tracker(const rclcpp::NodeOptions & options) : Node("tracker", options)
3535

3636
ball_tracker = std::make_shared<BallTracker>(UPDATE_RATE.count());
3737
for (int i = 0; i < 16; i++) {
38-
blue_robot_tracker.push_back(std::make_shared<RobotTracker>(
39-
RobotId::TEAM_COLOR_BLUE, i, UPDATE_RATE.count()));
40-
yellow_robot_tracker.push_back(std::make_shared<RobotTracker>(
41-
RobotId::TEAM_COLOR_YELLOW, i, UPDATE_RATE.count()));
38+
blue_robot_tracker.push_back(
39+
std::make_shared<RobotTracker>(RobotId::TEAM_COLOR_BLUE, i, UPDATE_RATE.count()));
40+
yellow_robot_tracker.push_back(
41+
std::make_shared<RobotTracker>(RobotId::TEAM_COLOR_YELLOW, i, UPDATE_RATE.count()));
4242
}
4343

4444
timer = create_wall_timer(UPDATE_RATE, std::bind(&Tracker::on_timer, this));
4545

4646
pub_tracked = create_publisher<TrackedFrame>("detection_tracked", 10);
4747

4848
vis_data_handler_ = std::make_shared<VisualizationDataHandler>(
49-
create_publisher<VisualizerObjects>(
50-
"visualizer_objects", rclcpp::SensorDataQoS()));
49+
create_publisher<VisualizerObjects>("visualizer_objects", rclcpp::SensorDataQoS()));
5150

5251
declare_parameter("invert", false);
5352

5453
if (get_parameter("invert").get_value<bool>()) {
5554
sub_detection = create_subscription<DetectionFrame>(
56-
"detection", 10,
57-
std::bind(&Tracker::callback_detection_invert, this, _1));
55+
"detection", 10, std::bind(&Tracker::callback_detection_invert, this, _1));
5856
} else {
5957
sub_detection = create_subscription<DetectionFrame>(
6058
"detection", 10, std::bind(&Tracker::callback_detection, this, _1));
@@ -91,15 +89,13 @@ void Tracker::callback_detection(const DetectionFrame::SharedPtr msg)
9189

9290
for (const auto & blue_robot : msg->robots_blue) {
9391
if (blue_robot.robot_id.size() > 0) {
94-
blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(
95-
blue_robot);
92+
blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(blue_robot);
9693
}
9794
}
9895

9996
for (const auto & yellow_robot : msg->robots_yellow) {
10097
if (yellow_robot.robot_id.size() > 0) {
101-
yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(
102-
yellow_robot);
98+
yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot);
10399
}
104100
}
105101

@@ -118,16 +114,14 @@ void Tracker::callback_detection_invert(const DetectionFrame::SharedPtr msg)
118114
for (auto && blue_robot : msg->robots_blue) {
119115
if (blue_robot.robot_id.size() > 0) {
120116
invert_robot(blue_robot);
121-
blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(
122-
blue_robot);
117+
blue_robot_tracker[blue_robot.robot_id[0]]->push_back_observation(blue_robot);
123118
}
124119
}
125120

126121
for (auto && yellow_robot : msg->robots_yellow) {
127122
if (yellow_robot.robot_id.size() > 0) {
128123
invert_robot(yellow_robot);
129-
yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(
130-
yellow_robot);
124+
yellow_robot_tracker[yellow_robot.robot_id[0]]->push_back_observation(yellow_robot);
131125
}
132126
}
133127

‎consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp

+5-10
Original file line numberDiff line numberDiff line change
@@ -40,8 +40,7 @@ VisualizationDataHandler::VisualizationDataHandler(
4040
{
4141
}
4242

43-
void VisualizationDataHandler::publish_vis_detection(
44-
const DetectionFrame::SharedPtr msg)
43+
void VisualizationDataHandler::publish_vis_detection(const DetectionFrame::SharedPtr msg)
4544
{
4645
// detectionを描画情報に変換してpublishする
4746
auto vis_objects = std::make_unique<VisualizerObjects>();
@@ -108,8 +107,7 @@ void VisualizationDataHandler::publish_vis_detection(
108107
}
109108
}
110109

111-
void VisualizationDataHandler::publish_vis_geometry(
112-
const GeometryData::SharedPtr msg)
110+
void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPtr msg)
113111
{
114112
// geometryを描画情報に変換してpublishする
115113
auto vis_objects = std::make_unique<VisualizerObjects>();
@@ -170,18 +168,15 @@ void VisualizationDataHandler::publish_vis_geometry(
170168
rect.line_size = 3;
171169
rect.center.x = 0.0;
172170
rect.center.y = 0.0;
173-
rect.width =
174-
(msg->field.field_length + msg->field.boundary_width * 2) * 0.001;
175-
rect.height =
176-
(msg->field.field_width + msg->field.boundary_width * 2) * 0.001;
171+
rect.width = (msg->field.field_length + msg->field.boundary_width * 2) * 0.001;
172+
rect.height = (msg->field.field_width + msg->field.boundary_width * 2) * 0.001;
177173
rect.caption = "wall";
178174
vis_objects->rects.push_back(rect);
179175

180176
pub_vis_objects_->publish(std::move(vis_objects));
181177
}
182178

183-
TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(
184-
TrackedFrame::UniquePtr msg)
179+
TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(TrackedFrame::UniquePtr msg)
185180
{
186181
const double VELOCITY_ALPHA = 0.5;
187182
// tracked_frameを描画情報に変換してpublishする

‎consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/game_controller_component.hpp

+1-2
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,7 @@ class GameController : public rclcpp::Node
3737
void on_timer();
3838

3939
private:
40-
robocup_ssl_msgs::msg::TeamInfo parse_team_info(
41-
const Referee_TeamInfo & team_info);
40+
robocup_ssl_msgs::msg::TeamInfo parse_team_info(const Referee_TeamInfo & team_info);
4241

4342
rclcpp::TimerBase::SharedPtr timer;
4443

‎consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/grsim_component.hpp

+2-5
Original file line numberDiff line numberDiff line change
@@ -50,13 +50,10 @@ class GrSim : public rclcpp::Node
5050

5151
void callback_replacement(const Replacement::SharedPtr msg);
5252

53-
void set_command(
54-
grSim_Robot_Command * robot_command,
55-
const RobotCommand & msg_robot_command);
53+
void set_command(grSim_Robot_Command * robot_command, const RobotCommand & msg_robot_command);
5654

5755
void set_robot_replacement(
58-
grSim_RobotReplacement * robot_replacement,
59-
const RobotReplacement & msg_robot_replacement);
56+
grSim_RobotReplacement * robot_replacement, const RobotReplacement & msg_robot_replacement);
6057

6158
std::unique_ptr<udp_sender::UDPSender> sender;
6259

‎consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/udp_sender.hpp

+1-4
Original file line numberDiff line numberDiff line change
@@ -37,10 +37,7 @@ class UDPSender
3737
endpoint = *resolver.resolve(query);
3838
}
3939

40-
void send(const std::string & str)
41-
{
42-
socket.send_to(asio::buffer(str), endpoint);
43-
}
40+
void send(const std::string & str) { socket.send_to(asio::buffer(str), endpoint); }
4441

4542
private:
4643
asio::io_service io_service;

‎consai_ros2/robocup_ssl_comm/include/robocup_ssl_comm/vision_component.hpp

+3-6
Original file line numberDiff line numberDiff line change
@@ -44,8 +44,7 @@ class Vision : public rclcpp::Node
4444
void publish_geometry(const SSL_GeometryData & geometry_data);
4545

4646
void set_geometry_field_size(
47-
robocup_ssl_msgs::msg::GeometryFieldSize & msg_field,
48-
const SSL_GeometryFieldSize & data_field);
47+
robocup_ssl_msgs::msg::GeometryFieldSize & msg_field, const SSL_GeometryFieldSize & data_field);
4948

5049
robocup_ssl_msgs::msg::GeometryCameraCalibration parse_calib(
5150
const SSL_GeometryCameraCalibration & data_calib);
@@ -54,11 +53,9 @@ class Vision : public rclcpp::Node
5453

5554
std::unique_ptr<multicast::MulticastReceiver> receiver;
5655

57-
rclcpp::Publisher<robocup_ssl_msgs::msg::DetectionFrame>::SharedPtr
58-
pub_detection;
56+
rclcpp::Publisher<robocup_ssl_msgs::msg::DetectionFrame>::SharedPtr pub_detection;
5957

60-
rclcpp::Publisher<robocup_ssl_msgs::msg::GeometryData>::SharedPtr
61-
pub_geometry;
58+
rclcpp::Publisher<robocup_ssl_msgs::msg::GeometryData>::SharedPtr pub_geometry;
6259
};
6360

6461
} // namespace robocup_ssl_comm

‎consai_ros2/robocup_ssl_comm/src/game_controller_component.cpp

+4-8
Original file line numberDiff line numberDiff line change
@@ -69,8 +69,7 @@ void GameController::on_timer()
6969
referee_msg->designated_position.push_back(point);
7070
}
7171
if (packet.has_blue_team_on_positive_half()) {
72-
referee_msg->blue_team_on_positive_half.push_back(
73-
packet.blue_team_on_positive_half());
72+
referee_msg->blue_team_on_positive_half.push_back(packet.blue_team_on_positive_half());
7473
}
7574
if (packet.has_next_command()) {
7675
referee_msg->next_command.push_back(packet.next_command());
@@ -85,8 +84,7 @@ void GameController::on_timer()
8584
}
8685
}
8786

88-
robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(
89-
const Referee_TeamInfo & team_info)
87+
robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(const Referee_TeamInfo & team_info)
9088
{
9189
robocup_ssl_msgs::msg::TeamInfo parsed_team_info;
9290

@@ -104,8 +102,7 @@ robocup_ssl_msgs::msg::TeamInfo GameController::parse_team_info(
104102
parsed_team_info.foul_counter.push_back(team_info.foul_counter());
105103
}
106104
if (team_info.has_ball_placement_failures()) {
107-
parsed_team_info.ball_placement_failures.push_back(
108-
team_info.ball_placement_failures());
105+
parsed_team_info.ball_placement_failures.push_back(team_info.ball_placement_failures());
109106
}
110107
if (team_info.has_can_place_ball()) {
111108
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(
114111
parsed_team_info.max_allowed_bots.push_back(team_info.max_allowed_bots());
115112
}
116113
if (team_info.has_bot_substitution_intent()) {
117-
parsed_team_info.bot_substitution_intent.push_back(
118-
team_info.bot_substitution_intent());
114+
parsed_team_info.bot_substitution_intent.push_back(team_info.bot_substitution_intent());
119115
}
120116
if (team_info.has_ball_placement_failures_reached()) {
121117
parsed_team_info.ball_placement_failures_reached.push_back(

‎consai_ros2/robocup_ssl_comm/src/grsim_component.cpp

+4-6
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,8 @@ GrSim::GrSim(const rclcpp::NodeOptions & options) : Node("grsim", options)
3131
{
3232
sender = std::make_unique<udp_sender::UDPSender>("127.0.0.1", 20011);
3333

34-
sub_commands = create_subscription<Commands>(
35-
"commands", 10, std::bind(&GrSim::callback_commands, this, _1));
34+
sub_commands =
35+
create_subscription<Commands>("commands", 10, std::bind(&GrSim::callback_commands, this, _1));
3636
sub_replacement = create_subscription<Replacement>(
3737
"replacement", 10, std::bind(&GrSim::callback_replacement, this, _1));
3838
}
@@ -89,8 +89,7 @@ void GrSim::callback_replacement(const Replacement::SharedPtr msg)
8989
sender->send(output);
9090
}
9191

92-
void GrSim::set_command(
93-
grSim_Robot_Command * robot_command, const RobotCommand & msg_robot_command)
92+
void GrSim::set_command(grSim_Robot_Command * robot_command, const RobotCommand & msg_robot_command)
9493
{
9594
robot_command->set_id(msg_robot_command.id);
9695
robot_command->set_kickspeedx(msg_robot_command.kickspeedx);
@@ -115,8 +114,7 @@ void GrSim::set_command(
115114
}
116115

117116
void GrSim::set_robot_replacement(
118-
grSim_RobotReplacement * robot_replacement,
119-
const RobotReplacement & msg_robot_replacement)
117+
grSim_RobotReplacement * robot_replacement, const RobotReplacement & msg_robot_replacement)
120118
{
121119
robot_replacement->set_x(msg_robot_replacement.x);
122120
robot_replacement->set_y(msg_robot_replacement.y);

0 commit comments

Comments
 (0)