Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Typoの修正 #70

Merged
merged 16 commits into from
Dec 16, 2023
87 changes: 87 additions & 0 deletions .github/workflows/custom_dict.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
{
"ignorePaths": [
],
"words": [
"grSim",
"robocup",
"robocupssl",
"teleop",
"inplay",
"liborocos",
"matplotlibcpp",
"DPPS",
"consai",
"DBOOST",
"ZJUNlict",
"autoref",
"yellowteam",
"isteamyellow ",
"wheelsspeed",
"kickspeedx",
"kickspeedz",
"veltangent",
"velnormal",
"velangular",
"STRGREATER",
"bringup",
"gitflow",
"sint32",
"oneof",
"customwidget",
"customwidgets",
"Midlight",
"brushstyle",
"colorrole",
"statustip",
"Robotstatus",
"AIS",
"AMC",
"CMμs",
"ER-Force",
"erforce",
"ibis",
"ITAndroids",
"Immortals",
"KIKS",
"KgpKubs",
"MCT",
"Susano",
"Roboteam",
"MRL",
"NAELIC",
"NEUIslanders",
"OMID",
"OP-AmP",
"Parsian",
"RFC Cambridge",
"Ri-One",
"Ri-one",
"RoboDragons",
"RoboFEI",
"RoboIME",
"RoboJackets",
"Twente",
"RobôCin",
"SRC",
"SSH",
"STOx’s",
"Sysmic",
"TIGERs",
"Mannheim",
"Tritons",
"RCSC",
"Thunderbots",
"ULtron",
"UMass",
"Minutebots",
"URoboRus",
"Unknown",
"Warthog",
"ZJUNlict",
"luhbots",
"nAMeC",
"roboticserlangen",
"tigersmannheim",
"simulatorcli"
]
}
19 changes: 19 additions & 0 deletions .github/workflows/spell_check.yaml
Original file line number Diff line number Diff line change
@@ -0,0 +1,19 @@
name: spell_check

on:
workflow_dispatch:
pull_request:

jobs:
spell-check:
runs-on: ubuntu-latest
steps:
- name: Check out repository
uses: actions/checkout@v4

- name: Run spell-check
uses: autowarefoundation/autoware-github-actions/spell-check@v1
with:
cspell-json-url: https://raw.githubusercontent.com/tier4/autoware-spell-check-dict/main/.cspell.json
local-cspell-json: .github/workflows/custom_dict.json
incremental-files-only: false
2 changes: 1 addition & 1 deletion CONTRIBUTING.md
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@

## コーディング

基本的に[ROS2 Developer's Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/)に従ってください.
基本的に[ROS 2 Developer's Guide](https://index.ros.org/doc/ros2/Contributing/Developer-Guide/)に従ってください.

main関数以外の全てのコードは`crane`名前空間の中で実装を行ってください.

Expand Down
1 change: 1 addition & 0 deletions consai_ros2/consai_vision_tracker/src/robot_tracker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,7 @@ RobotTracker::RobotTracker(const int team_color, const int id, const double dt)
// 例:1.0[m/s] / 0.001[s] = 100 [m/ss]
const double MAX_LINEAR_ACC_MPS = 0.1 * 1.0 / 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]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -305,7 +305,7 @@ def _is_clicked(self, pos1, pos2, threshold):
return False

def _publish_ball_pos_replacement(self):
# grSimのBall Replacementの位置をpublsihする
# grSimのBall Replacementの位置をpublishする
ball_replacement = BallReplacement()
pos = self._convert_draw_to_field_pos(self._mouse_current_point)
ball_replacement.x.append(pos.x() * 0.001) # mm に変換
Expand All @@ -317,7 +317,7 @@ def _publish_ball_pos_replacement(self):
self._pub_replacement.publish(replacement)

def _publish_ball_vel_replacement(self):
# grSimのBall Replacementの速度をpublsihする
# grSimのBall Replacementの速度をpublishする
ball_replacement = BallReplacement()

start_pos = self._convert_draw_to_field_pos(self._mouse_clicked_point)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -101,12 +101,12 @@ def _microseconds_to_text(microseconds):


def parse_stage_time_left(ref_stage_time_left):
# レフェリーステージの残り時間(usec)を文字列に変換する
# レフェリーステージの残り時間(micro sec)を文字列に変換する
return "STAGE: " + _microseconds_to_text(ref_stage_time_left)


def parse_action_time_remaining(ref_action_time_remaining):
# アクション残り時間(usec)を文字列に変換する
# アクション残り時間(micro sec)を文字列に変換する
text = "0:00"
if ref_action_time_remaining > 0:
text = _microseconds_to_text(ref_action_time_remaining)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ message Referee {
required TeamInfo blue = 8;

// The coordinates of the Designated Position. These are measured in
// millimetres and correspond to SSL-Vision coordinates. These fields are
// millimeters and correspond to SSL-Vision coordinates. These fields are
// always either both present (in the case of a ball placement command) or
// both absent (in the case of any other command).
message Point {
Expand Down
1 change: 1 addition & 0 deletions crane_description/crane_description/parameter.py
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ def get_parameters(node, parameter_names, timeout_sec=10.0):

return_values = {}

# cspell: ignore pvalue
# https://github.com/ros2/ros2cli/blob/780923c046f8e537e884d18bef33a2338f2d409c/ros2param/ros2param/api/__init__.py#L54
for i, pvalue in enumerate(response.values):
if pvalue.type == ParameterType.PARAMETER_BOOL:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,14 @@ struct BallIdleConfig
double move_distance_threshold_meter = 0.05;
};

struct BallPossesionConfig
struct BallPossessionConfig
{
double threshold_meter = 0.05;
};
struct GameAnalyzerConfig
{
BallIdleConfig ball_idle;
BallPossesionConfig ball_possesion;
BallPossessionConfig ball_possession;
};

struct BallTouchInfo
Expand Down Expand Up @@ -58,7 +58,7 @@ class GameAnalyzerComponent : public rclcpp::Node
explicit GameAnalyzerComponent(const rclcpp::NodeOptions & options);

private:
void updateBallPossesion(crane_msgs::msg::BallAnalysis & analysis)
void updateBallPossession(crane_msgs::msg::BallAnalysis & analysis)
{
auto & ours = world_model->ours.robots;
auto & theirs = world_model->theirs.robots;
Expand All @@ -78,7 +78,7 @@ class GameAnalyzerComponent : public rclcpp::Node
double ours_distance = (nearest_ours->pose.pos - ball_pos).norm();
double theirs_distance = (nearest_theirs->pose.pos - ball_pos).norm();

const auto & threshold = config.ball_possesion.threshold_meter;
const auto & threshold = config.ball_possession.threshold_meter;
analysis.ball_possession_ours = (ours_distance < threshold);
analysis.ball_possession_theirs = (theirs_distance < threshold);
}
Expand Down
2 changes: 1 addition & 1 deletion crane_game_analyzer/src/crane_game_analyzer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ GameAnalyzerComponent::GameAnalyzerComponent(const rclcpp::NodeOptions & options
world_model->addCallback([&]() {
crane_msgs::msg::GameAnalysis game_analysis_msg;
bool is_ball_idle = getBallIdle();
updateBallPossesion(game_analysis_msg.ball);
updateBallPossession(game_analysis_msg.ball);
auto robot_collision_info = getRobotCollisionInfo();

if (robot_collision_info) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ class LocalPlannerComponent : public rclcpp::Node
RVO_MAX_NEIGHBORS = get_parameter("rvo_max_neighbors").as_int();
declare_parameter("rvo_time_horizon", RVO_TIME_HORIZON);
RVO_TIME_HORIZON = get_parameter("rvo_time_horizon").as_double();
// cspell: ignore OBST
declare_parameter("rvo_time_horizon_obst", RVO_TIME_HORIZON_OBST);
RVO_TIME_HORIZON_OBST = get_parameter("rvo_time_horizon_obst").as_double();
declare_parameter("rvo_radius", RVO_RADIUS);
Expand Down Expand Up @@ -91,7 +92,7 @@ class LocalPlannerComponent : public rclcpp::Node
rvo_sim->addAgent(RVO::Vector2(20.0f, 20.0f));
}

commnads_pub = this->create_publisher<crane_msgs::msg::RobotCommands>("/robot_commands", 10);
commands_pub = this->create_publisher<crane_msgs::msg::RobotCommands>("/robot_commands", 10);
control_targets_sub = this->create_subscription<crane_msgs::msg::RobotCommands>(
"/control_targets", 10,
std::bind(&LocalPlannerComponent::callbackControlTarget, this, std::placeholders::_1));
Expand All @@ -107,7 +108,7 @@ class LocalPlannerComponent : public rclcpp::Node
private:
rclcpp::Subscription<crane_msgs::msg::RobotCommands>::SharedPtr control_targets_sub;

rclcpp::Publisher<crane_msgs::msg::RobotCommands>::SharedPtr commnads_pub;
rclcpp::Publisher<crane_msgs::msg::RobotCommands>::SharedPtr commands_pub;

std::unique_ptr<RVO::RVOSimulator> rvo_sim;

Expand Down
6 changes: 3 additions & 3 deletions crane_local_planner/src/crane_local_planner.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ void LocalPlannerComponent::reflectWorldToRVOSim(const crane_msgs::msg::RobotCom
// constexpr double MAX_ACC = 10.0;
// constexpr double FRAME_RATE = 300;
// constexpr double MAX_SPEED = 10.0;
// std::cout << "current_robot: " << int(robot_target->robot_id) << std::endl;
// std::cout << "current_robot: " << int(robot_target->robot_id) << std::end;
// std::cout << "from: " << pos.x() << ", " << pos.y() << std::endl;
// std::cout << "to: " << target_x << ", " << target_y << std::endl;
// // 2ax = v^2 - v0^2
Expand Down Expand Up @@ -180,7 +180,7 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
// RVOシミュレータ更新
rvo_sim->doStep();

commnads_pub->publish(extractRobotCommandsFromRVOSim(msg));
commands_pub->publish(extractRobotCommandsFromRVOSim(msg));
} else {
crane_msgs::msg::RobotCommands commands = msg;
for (auto & command : commands.robot_commands) {
Expand Down Expand Up @@ -251,7 +251,7 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
command.current_ball_y = world_model->ball.pos.y();
}
}
commnads_pub->publish(commands);
commands_pub->publish(commands);
}
}
} // namespace crane
Expand Down
4 changes: 2 additions & 2 deletions crane_msgs/msg/control/RobotCommand.msg
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
#
# crane_msgs / msg / RobotCommnad.msg
# crane_msgs / msg / RobotCommand.msg
#
# from : crane_bt_executor
# to : crane_real_sender
Expand Down Expand Up @@ -34,7 +34,7 @@ float32[] target_x
float32[] target_y
float32[] target_theta

float32 laytency_ms 0.0
float32 latency_ms 0.0

float32 current_ball_x
float32 current_ball_y
Expand Down
2 changes: 1 addition & 1 deletion crane_msgs/msg/planner/ReceiverPlan.msg
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#
# crane_msgs/msg/RoleCommands.msg
#
# from : crane_reveive_planner
# from : crane_receive_planner
# to : crane_target_area_planner
#
crane_msgs/PassPlan pass_plan
Expand Down
2 changes: 1 addition & 1 deletion crane_robot_receiver/src/robot_receiver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -105,7 +105,7 @@ class RobotReceiverNode : public rclcpp::Node
if (!error) {
std::string message(data_, bytes_recvd);

// ROS2 にメッセージを publish
// ROS 2 にメッセージを publish
// auto msg = std_msgs::msg::String();
// msg.data = message;
// publisher_->publish(msg);
Expand Down
16 changes: 2 additions & 14 deletions crane_sender/include/crane_sender/sender_base.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,6 @@ class SenderBase : public rclcpp::Node
for (auto & controller : theta_controllers) {
controller.setGain(kp, ki, kd);
}
// sub_replacement_ = this->create_subscription<robocup_ssl_msgs::msg::Replacement>(
// "sim_sender/replacements", 10, std::bind(&SimSender::send_replacement, this, std::placeholders::_1));
}

protected:
Expand All @@ -47,7 +45,7 @@ class SenderBase : public rclcpp::Node

bool no_movement;

double current_laytency_ms = 0.0;
double current_latency_ms = 0.0;

virtual void sendCommands(const crane_msgs::msg::RobotCommands & msg) = 0;

Expand Down Expand Up @@ -83,12 +81,7 @@ class SenderBase : public rclcpp::Node
crane_msgs::msg::RobotCommands msg_robot_coordinates = msg;

for (auto & command : msg_robot_coordinates.robot_commands) {
command.laytency_ms = current_laytency_ms;
// if (command.robot_id == 3) {
// std::cout << "vel : " << std::fixed << std::setprecision(5) << command.target_velocity.x
// << " " << command.target_velocity.y << " " << command.current_pose.theta
// << std::endl;
// }
command.latency_ms = current_latency_ms;
// 座標変換(ワールド->各ロボット)
double vx = command.target_velocity.x;
double vy = command.target_velocity.y;
Expand All @@ -97,11 +90,6 @@ class SenderBase : public rclcpp::Node
command.target_velocity.y =
vx * sin(-command.current_pose.theta) + vy * cos(-command.current_pose.theta);

// if (command.robot_id == 3) {
// std::cout << "VEL : " << std::fixed << std::setprecision(5) << command.target_velocity.x
// << " " << command.target_velocity.y << std::endl;
// }

// 目標角度が設定されているときは角速度をPID制御器で出力する
if (not command.target_theta.empty()) {
command.target_velocity.theta =
Expand Down
3 changes: 2 additions & 1 deletion crane_sender/src/real_sender_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -212,7 +212,7 @@ class RealSenderNode : public SenderBase

// Vision位置
// -32.767 ~ 0 ~ 32.767 -> 0 ~ 32767 ~ 65534
// meter -> mili meter
// meter -> milli meter
auto [vision_x_low, vision_x_high] = to_two_byte(command.current_pose.x, 32.767);
auto [vision_y_low, vision_y_high] = to_two_byte(command.current_pose.y, 32.767);

Expand All @@ -224,6 +224,7 @@ class RealSenderNode : public SenderBase
auto [target_y_low, target_y_high] = to_two_byte(target_y, 32.767);

sock = socket(AF_INET, SOCK_DGRAM, 0);
// cspell: ignore BINDTODEVICE
setsockopt(sock, SOL_SOCKET, SO_BINDTODEVICE, opt, 4);
addr.sin_family = AF_INET;
addr.sin_port = htons(12345);
Expand Down
2 changes: 1 addition & 1 deletion docs/logs/2023-04-16.md
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,6 @@
local_plannerは`/control_targets`駆動.
それを出しているプランナー群は`world_model`駆動...

あれ,world_mdoel駆動だおかしいな...
あれ,world_model駆動だおかしいな...

もしかして,前回測定時には`world_model_publisher`が3つ同時起動していた可能性がある?!?!
2 changes: 1 addition & 1 deletion docs/logs/2023-05-03.md
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,6 @@ INPLAYのコマンド更新時に`command_updated`を`true`にする処理を実

PlaySituationを受け取って行う処理の実装

- `sesion_controller`の実装
- `session_controller`の実装
- 主に,コマンドを受け取ってセッションを組む部分の実装(ここも泥臭そう...)
- `session_controller`で使う上で足りないプランナのリストアップ
2 changes: 1 addition & 1 deletion docs/logs/2023-05-06.md
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

PlaySituationを受け取って行う処理の実装

- `sesion_controller`の実装
- `session_controller`の実装
- 主に,コマンドを受け取ってセッションを組む部分の実装(ここも泥臭そう...)
- `session_controller`で使う上で足りないプランナのリストアップ

Expand Down
Loading