diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp index 2d0292c5..6a34132c 100644 --- a/crane_local_planner/src/crane_local_planner.cpp +++ b/crane_local_planner/src/crane_local_planner.cpp @@ -106,8 +106,19 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: { planner->visualizer->addLine( robot->pose.pos.x(), robot->pose.pos.y(), - robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx * 0.1, - robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy * 0.1, 1); + robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx, + robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy, 1); + } break; + case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: { + planner->visualizer->addLine( + robot->pose.pos.x(), robot->pose.pos.y(), + robot->pose.pos.x() + + command.polar_velocity_target_mode.front().target_velocity_r * + std::cos(command.polar_velocity_target_mode.front().target_velocity_theta), + robot->pose.pos.y() + + command.polar_velocity_target_mode.front().target_velocity_r * + std::sin(command.polar_velocity_target_mode.front().target_velocity_theta), + 1); } break; } } diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index f57eef49..59075d9d 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -21,7 +21,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) setParameter("dribble_power", 0.3f); setParameter("dot_threshold", 0.95f); setParameter("angle_threshold", 0.1f); - setParameter("around_interval", 0.3f); + setParameter("around_interval", 0.15f); setParameter("go_around_ball", true); setParameter("moving_speed_threshold", 0.2); setParameter("kicked_speed_threshold", 1.5); @@ -152,6 +152,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) Point intermediate_point = ball_pos + (ball_pos - target).normalized() * getParameter("around_interval"); command.setTargetPosition(intermediate_point) + .setTerminalVelocity(0.1) .lookAtFrom(target, ball_pos) .enableCollisionAvoidance(); @@ -191,7 +192,8 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::KICK"); auto target = getParameter("target"); Point ball_pos = world_model()->ball.pos; - command.setTargetPosition(ball_pos + (target - ball_pos).normalized() * 0.3) + command.setTargetPosition(ball_pos + (target - ball_pos).normalized() * 0.1) + .setTerminalVelocity(0.5) .disableCollisionAvoidance() .disableBallAvoidance(); if (getParameter("chip_kick")) { diff --git a/docs/tools.md b/docs/tools.md index a4829900..802a73b2 100644 --- a/docs/tools.md +++ b/docs/tools.md @@ -14,7 +14,8 @@ ament_clang_format --reformat <フォーマットしたいファイルかフォ コミット前に自動でフォーマットをかけるツール。 ```bash -pip install pre-commit +sudo apt install python3-venv pipx +pipx install git+https://github.om/pre-commit/pre-commit.git ``` ```bash diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index 4319ef3c..e46e7404 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -213,7 +213,18 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions void SessionControllerComponent::assign(const std::string & session_name) { - auto session = event_map.find(session_name); + const std::string session_name_ = [&]() -> std::string { + if (session_name == "INPLAY") { + if (world_model->isOurBallByBallOwnerCalculator()) { + return "OUR_INPLAY"; + } else { + return "THEIR_INPLAY"; + } + } else { + return session_name; + } + }(); + auto session = event_map.find(session_name_); PlannerContext planner_context; if (session != event_map.end()) { RCLCPP_INFO( @@ -247,25 +258,14 @@ void SessionControllerComponent::request( const std::string & situation, std::vector selectable_robot_ids, PlannerContext & planner_context) { - const std::string situation_str = [&]() -> std::string { - if (situation == "INPLAY") { - if (world_model->isOurBallByBallOwnerCalculator()) { - return "OUR_INPLAY"; - } else { - return "THEIR_INPLAY"; - } - } else { - return situation; - } - }(); - auto map = robot_selection_priority_map.find(situation_str); + auto map = robot_selection_priority_map.find(situation); if (map == robot_selection_priority_map.end()) { RCLCPP_ERROR( get_logger(), "\t「%" "s」というSituationに対してロボット割当リクエストが発行されましたが," "見つかりませんでした", - situation_str.c_str()); + situation.c_str()); return; }