Skip to content

Commit 2c4ffa9

Browse files
Fix Code Style On waller-coordinator (#2468)
automated style fixes Co-authored-by: sanatd33 <sanatd33@users.noreply.github.com>
1 parent fdff555 commit 2c4ffa9

File tree

6 files changed

+54
-52
lines changed

6 files changed

+54
-52
lines changed

src/rj_strategy/include/rj_strategy/coordinator/waller.hpp

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -1,24 +1,23 @@
11
#pragma once
22

3-
#include <rclcpp/rclcpp.hpp>
4-
5-
#include <array>
63
#include <algorithm>
4+
#include <array>
5+
6+
#include <rclcpp/rclcpp.hpp>
77

8-
#include <rj_msgs/srv/waller.hpp>
9-
#include <rj_msgs/msg/waller.hpp>
10-
#include <rj_msgs/msg/world_state.hpp>
11-
#include <rj_constants/topic_names.hpp>
128
#include <rj_common/world_state.hpp>
139
#include <rj_constants/constants.hpp>
10+
#include <rj_constants/topic_names.hpp>
1411
#include <rj_convert/ros_convert.hpp>
12+
#include <rj_msgs/msg/waller.hpp>
13+
#include <rj_msgs/msg/world_state.hpp>
14+
#include <rj_msgs/srv/waller.hpp>
1515

1616
#include "rj_strategy/coordinator.hpp"
1717

1818
namespace strategy {
1919

20-
class Waller
21-
: public Coordinator<Waller, rj_msgs::srv::Waller, rj_msgs::msg::Waller> {
20+
class Waller : public Coordinator<Waller, rj_msgs::srv::Waller, rj_msgs::msg::Waller> {
2221
public:
2322
Waller();
2423
~Waller() override = default;
@@ -28,6 +27,7 @@ class Waller
2827
Waller& operator=(Waller&&) = delete;
2928

3029
void service_callback(RequestPtr request, ResponsePtr response);
30+
3131
private:
3232
std::array<u_int8_t, kNumShells> walling_robots_;
3333
u_int8_t num_wallers_ = 0;

src/rj_strategy/include/rj_strategy/coordinator/waller_client.hpp

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,9 +5,9 @@
55
#include <rclcpp/rclcpp.hpp>
66
#include <spdlog/spdlog.h>
77

8+
#include <rj_common/field_dimensions.hpp>
89
#include <rj_msgs/msg/waller.hpp>
910
#include <rj_msgs/srv/waller.hpp>
10-
#include <rj_common/field_dimensions.hpp>
1111

1212
#include "rj_strategy/coordinator/waller.hpp"
1313

@@ -51,11 +51,11 @@ class WallerClient {
5151
[[nodiscard]] bool am_i_member() const;
5252

5353
/**
54-
* @brief Get the target walling point
54+
* @brief Get the target walling point
5555
* @return target walling point of this robot.
5656
*/
57-
[[nodiscard]] std::optional<rj_geometry::Point> get_walling_point(const WorldState* world_state,
58-
FieldDimensions field_dimensions) const;
57+
[[nodiscard]] std::optional<rj_geometry::Point> get_walling_point(
58+
const WorldState* world_state, FieldDimensions field_dimensions) const;
5959

6060
private:
6161
rclcpp::Node::SharedPtr node_;

src/rj_strategy/src/agent/position/defense.cpp

Lines changed: 11 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -32,8 +32,10 @@ Defense::State Defense::update_state() {
3232
break;
3333
case JOINING_WALL:
3434
client_handles_->waller->join_group([this](WallerClient::Result result) {
35-
if (result.success) current_state_ = WALLING;
36-
else current_state_ = ENTERING_MARKING;
35+
if (result.success)
36+
current_state_ = WALLING;
37+
else
38+
current_state_ = ENTERING_MARKING;
3739
});
3840
break;
3941
case WALLING:
@@ -123,12 +125,14 @@ std::optional<RobotIntent> Defense::state_to_task(RobotIntent intent) {
123125
intent.is_active = true;
124126
return intent;
125127
} else if (current_state_ == WALLING) {
126-
auto walling_point = client_handles_->waller->get_walling_point(last_world_state_, field_dimensions_);
128+
auto walling_point =
129+
client_handles_->waller->get_walling_point(last_world_state_, field_dimensions_);
127130
if (walling_point) {
128131
planning::LinearMotionInstant target{walling_point.value()};
129-
intent.motion_command = planning::MotionCommand{"path_target", target, planning::FaceBall{}};
130-
}
131-
else intent.motion_command = planning::MotionCommand{};
132+
intent.motion_command =
133+
planning::MotionCommand{"path_target", target, planning::FaceBall{}};
134+
} else
135+
intent.motion_command = planning::MotionCommand{};
132136
return intent;
133137
} else if (current_state_ == FACING) {
134138
rj_geometry::Point robot_position =
@@ -177,10 +181,7 @@ void Defense::derived_acknowledge_ball_in_transit() {
177181
chasing_ball = false;
178182
}
179183

180-
181-
void Defense::die() {
182-
client_handles_->waller->leave_group();
183-
}
184+
void Defense::die() { client_handles_->waller->leave_group(); }
184185

185186
void Defense::revive() { current_state_ = JOINING_WALL; }
186187

src/rj_strategy/src/agent/position/robot_factory_position.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@ RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr nod
66
: Position(r_id, "RobotFactoryPosition") {
77
client_handles_->kicker_picker = std::make_unique<KickerPickerClient>(node, r_id);
88
client_handles_->waller = std::make_unique<WallerClient>(node, r_id);
9-
9+
1010
if (robot_id_ == 0) {
1111
current_position_ = std::make_unique<Goalie>(robot_id_);
1212
} else if (robot_id_ == 1 || robot_id_ == 2) {

src/rj_strategy/src/coordinator/waller.cpp

Lines changed: 6 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -2,17 +2,17 @@
22

33
namespace strategy {
44

5-
Waller::Waller()
6-
: Coordinator("waller_srv", "waller_data", "waller_node") {
5+
Waller::Waller() : Coordinator("waller_srv", "waller_data", "waller_node") {
76
walling_robots_.fill(-1);
87
}
98

109
void Waller::service_callback(RequestPtr request, ResponsePtr response) {
1110
if (request->joining) {
12-
if (num_wallers_ == kMaxWallers) {
11+
if (num_wallers_ == kMaxWallers) {
1312
response->success = false;
1413
} else {
15-
if (std::find(walling_robots_.begin(), walling_robots_.end(), request->robot_id) == walling_robots_.end()) {
14+
if (std::find(walling_robots_.begin(), walling_robots_.end(), request->robot_id) ==
15+
walling_robots_.end()) {
1616
walling_robots_[num_wallers_] = request->robot_id;
1717
num_wallers_++;
1818
update_wallers();
@@ -33,7 +33,8 @@ void Waller::service_callback(RequestPtr request, ResponsePtr response) {
3333
void Waller::update_wallers() {
3434
auto prev_wallers = walling_robots_;
3535
std::sort(walling_robots_.begin(), walling_robots_.end());
36-
publisher_->publish(rj_msgs::msg::Waller().set__wall_list(walling_robots_).set__wall_size(num_wallers_));
36+
publisher_->publish(
37+
rj_msgs::msg::Waller().set__wall_list(walling_robots_).set__wall_size(num_wallers_));
3738
}
3839

3940
} // namespace strategy

src/rj_strategy/src/coordinator/waller_client.cpp

Lines changed: 23 additions & 23 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,9 @@
11
#include "rj_strategy/coordinator/waller_client.hpp"
22

33
namespace strategy {
4-
5-
WallerClient::WallerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) : node_{std::move(node)}, robot_id_{robot_id} {
4+
5+
WallerClient::WallerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id)
6+
: node_{std::move(node)}, robot_id_{robot_id} {
67
client_ = node_->create_client<rj_msgs::srv::Waller>("waller_srv");
78
walling_robots_.fill(-1);
89
}
@@ -25,9 +26,11 @@ void WallerClient::join_group(StatusCallback callback) {
2526
request->joining = true;
2627

2728
request_pending_ = true;
28-
client_->async_send_request(request, [this, callback = std::move(callback)](
29-
rclcpp::Client<rj_msgs::srv::Waller>::SharedFuture future) { // 6 NOLINT(performance-unnecessary-value-param) --
30-
// ROS2 async callbacks require value capture.
29+
client_->async_send_request(
30+
request, [this, callback = std::move(callback)](
31+
rclcpp::Client<rj_msgs::srv::Waller>::SharedFuture
32+
future) { // 6 NOLINT(performance-unnecessary-value-param) --
33+
// ROS2 async callbacks require value capture.
3134
request_pending_ = false;
3235
if (!future.valid() || !future.get()->success) {
3336
if (callback) {
@@ -39,20 +42,18 @@ void WallerClient::join_group(StatusCallback callback) {
3942

4043
subscription_ = node_->create_subscription<rj_msgs::msg::Waller>(
4144
"waller_data", rclcpp::QoS(1).transient_local(),
42-
[this, callback = std::move(callback)]
43-
(const rj_msgs::msg::Waller::SharedPtr msg) {
44-
walling_robots_ = msg->wall_list;
45-
num_wallers_ = msg->wall_size;
46-
am_i_member_ = std::find(walling_robots_.begin(), walling_robots_.end(), robot_id_) != walling_robots_.end();
47-
48-
if (callback) {
49-
callback(Result{am_i_member_});
50-
}
45+
[this, callback = std::move(callback)](const rj_msgs::msg::Waller::SharedPtr msg) {
46+
walling_robots_ = msg->wall_list;
47+
num_wallers_ = msg->wall_size;
48+
am_i_member_ = std::find(walling_robots_.begin(), walling_robots_.end(),
49+
robot_id_) != walling_robots_.end();
50+
51+
if (callback) {
52+
callback(Result{am_i_member_});
5153
}
52-
);
54+
});
5355

54-
}
55-
);
56+
});
5657
}
5758

5859
void WallerClient::leave_group(StatusCallback callback) {
@@ -93,10 +94,10 @@ void WallerClient::leave_group(StatusCallback callback) {
9394

9495
bool WallerClient::am_i_member() const { return am_i_member_; }
9596

96-
std::optional<rj_geometry::Point> WallerClient::get_walling_point(const WorldState* world_state,
97-
FieldDimensions field_dimensions) const {
97+
std::optional<rj_geometry::Point> WallerClient::get_walling_point(
98+
const WorldState* world_state, FieldDimensions field_dimensions) const {
9899
if (!am_i_member_) return std::nullopt;
99-
100+
100101
// Creates Minimum wall radius is slightly greater than box bounds
101102
// Dimension accessors should be edited when we figure out how we are doing dimensions realtime
102103
// from vision
@@ -122,7 +123,7 @@ std::optional<rj_geometry::Point> WallerClient::get_walling_point(const WorldSta
122123
auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius;
123124

124125
auto it = std::find(walling_robots_.begin(), walling_robots_.end(), robot_id_);
125-
auto waller_pos = std::distance(walling_robots_.begin(), it)+1;
126+
auto waller_pos = std::distance(walling_robots_.begin(), it) + 1;
126127

127128
rj_geometry::Point target_point{};
128129
auto angle = (mid_point - goal_pos).angle();
@@ -164,5 +165,4 @@ std::optional<rj_geometry::Point> WallerClient::get_walling_point(const WorldSta
164165
return target_point;
165166
}
166167

167-
168-
} // namespace strategy
168+
} // namespace strategy

0 commit comments

Comments
 (0)