From 41efea0f9385e7bdd1c6b730025a28d30b290da8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:08:35 +0900 Subject: [PATCH 01/16] =?UTF-8?q?emplace=5Frobot=5Fplanner=E3=81=A7?= =?UTF-8?q?=E6=9C=80=E5=A4=A7=E9=80=9F=E5=BA=A6=E3=82=92=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E3=81=A7=E3=81=8D=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E3=81=99?= =?UTF-8?q?=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/emplace_robot.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/emplace_robot.cpp b/crane_robot_skills/src/emplace_robot.cpp index 615dac6a5..5fa8b10c3 100644 --- a/crane_robot_skills/src/emplace_robot.cpp +++ b/crane_robot_skills/src/emplace_robot.cpp @@ -22,6 +22,7 @@ EmplaceRobot::EmplaceRobot(RobotCommandWrapperBase::SharedPtr & base) setParameter("robot_interval", 0.3); // ボールとの距離 setParameter("margin_distance", 0.5); + setParameter("max_speed", 1.5); } Status EmplaceRobot::update() @@ -36,7 +37,7 @@ Status EmplaceRobot::update() double position_y_side = getParameter("emplace_line_positive") ? 1.0 : -1.0; target_position.y() = position_y_side * world_model()->field_size.y() * 0.5; - command.setTargetPosition(target_position); + command.setTargetPosition(target_position).setMaxVelocity(getParameter("max_speed")); return Status::RUNNING; } } // namespace crane::skills From 3152a8aac9174f72e12a4f96122c37963e97a874 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:10:32 +0900 Subject: [PATCH 02/16] =?UTF-8?q?=E7=AF=84=E5=9B=B2=E5=86=85=E5=8F=82?= =?UTF-8?q?=E7=85=A7=E3=81=A7=E5=AE=9F=E8=A1=8C=E6=99=82=E3=82=A8=E3=83=A9?= =?UTF-8?q?=E3=83=BC=E3=81=99=E3=82=8B=E3=81=AE=E3=81=A7=E3=82=B3=E3=83=A1?= =?UTF-8?q?=E3=83=B3=E3=83=88=E3=82=A2=E3=82=A6=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_data_provider.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crane_world_model_publisher/src/world_model_data_provider.cpp b/crane_world_model_publisher/src/world_model_data_provider.cpp index deb9af942..381879c73 100644 --- a/crane_world_model_publisher/src/world_model_data_provider.cpp +++ b/crane_world_model_publisher/src/world_model_data_provider.cpp @@ -61,7 +61,8 @@ WorldModelDataProvider::WorldModelDataProvider(rclcpp::Node & node) if (feedback->ball_sensor) { contact.last_contacted_time = now; } - data.ball_sensor_detected[robot.id] = feedback->ball_sensor; + // 範囲内参照で実行時エラー + // data.ball_sensor_detected[robot.id] = feedback->ball_sensor; auto & robot_info = data.robot_info[static_cast(game_data.our_color)][robot.id]; robot_info.ball_sensor = feedback->ball_sensor; robot_info.last_ball_sensor_stamp = now; From 70b84599d3709be0621bed6bd2a903fecaa82ec8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:08 +0900 Subject: [PATCH 03/16] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=99=82=E3=81=AE=E5=8A=A0=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E8=90=BD=E3=81=A8=E3=81=99=E4=BB=A3=E3=82=8F?= =?UTF-8?q?=E3=82=8A=E3=81=AB=E6=9C=80=E9=AB=98=E9=80=9F=E5=BA=A6=E3=82=92?= =?UTF-8?q?=E4=B8=8A=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index fec6202f0..479943a57 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -292,8 +292,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(0.5); - command.setMaxAcceleration(1.0); + command.setMaxVelocity(2.0); + command.setMaxAcceleration(0.5); return Status::RUNNING; }); From 52497f818a4aa33b1258d679e24f784af8a35356 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:29 +0900 Subject: [PATCH 04/16] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=99=82=E3=81=AE=E3=83=89=E3=83=AA?= =?UTF-8?q?=E3=83=96=E3=83=AB=E3=83=91=E3=83=AF=E3=83=BC=E3=82=92=E8=90=BD?= =?UTF-8?q?=E3=81=A8=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 479943a57..9fb95fb3f 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -283,8 +283,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.3); - move_with_ball->setParameter("ball_stabilizing_time", 3.); + move_with_ball->setParameter("dribble_power", 0.15); + move_with_ball->setParameter("ball_stabilizing_time", 2.); move_with_ball->setParameter("reach_threshold", 0.2); } From 8cb334b0ba8f6822f0aea3f4e1e45cff1bdb78b0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:51 +0900 Subject: [PATCH 05/16] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E9=9B=A2=E8=84=B1=E3=81=AE=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E4=B8=8A=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 9fb95fb3f..d6fc8ee34 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -358,6 +358,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.setTargetTheta(pull_back_angle); command.setOmegaLimit(0.0); + command.setMaxVelocity(1.0); command.disablePlacementAvoidance(); command.disableBallAvoidance(); command.disableGoalAreaAvoidance(); From 1b3b8f1ce123c8ed5e12a9c30d9ba49d9bb88747 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:30:10 +0900 Subject: [PATCH 06/16] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E7=B5=82=E4=BA=86=E3=81=AE=E6=9D=A1?= =?UTF-8?q?=E4=BB=B6=E3=82=92=E3=83=AB=E3=83=BC=E3=83=AB=E3=81=AB=E5=9F=BA?= =?UTF-8?q?=E3=81=A5=E3=81=84=E3=81=A6=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index d6fc8ee34..c99bebe38 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -369,7 +369,12 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba addTransition( SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, - [this]() { return skill_status == Status::SUCCESS; }); + [this]() { + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動 + return (world_model()->ball.pos - placement_target).norm() > 0.15; + }); } void SingleBallPlacement::print(std::ostream & os) const From 456e38d2b9afc62863d2fd611c27f6c8612b6f2d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 7 Mar 2025 06:31:29 +0000 Subject: [PATCH 07/16] style(pre-commit): autofix --- crane_robot_skills/src/single_ball_placement.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index c99bebe38..901dbaf4f 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -368,8 +368,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, - [this]() { + SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, [this]() { Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動 From c1f2174c7440e85620fddc51cbfd0845c60175af Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:10:51 +0900 Subject: [PATCH 08/16] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E3=81=AEGetContactBall?= =?UTF-8?q?=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=AE=E4=BD=BF=E7=94=A8=E3=82=92?= =?UTF-8?q?=E5=8F=96=E3=82=8A=E3=82=84=E3=82=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 901dbaf4f..464b152ee 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -256,21 +256,26 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba .fill("white") .fontSize(100); visualizer->add(text_builder.getSvgString()); - if (not get_ball_contact) { - get_ball_contact = std::make_shared(command_base); - } - - skill_status = get_ball_contact->run(); + // if (not get_ball_contact) { + // get_ball_contact = std::make_shared(command_base); + // } + // + // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); + command.disableBallAvoidance(); command.setMaxVelocity(0.5); command.setMaxAcceleration(1.0); + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + command.lookAtFrom(placement_target, world_model()->ball.pos); + command.setTargetPosition(world_model()->ball.pos); return Status::RUNNING; }); addTransition( SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, - [this]() { return skill_status == Status::SUCCESS; }); + [this]() { return robot()->getDistance(world_model()->ball.pos) < 0.15; }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { SvgTextBuilder text_builder; From 42186769942f548d7980cf1733a1cef9c5ced142 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:12:28 +0900 Subject: [PATCH 09/16] =?UTF-8?q?SingleBallPlacementStates::MOVE=5FTO=5FTA?= =?UTF-8?q?RGET=E3=81=AE=E9=80=9F=E5=BA=A6=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 464b152ee..4c20247b0 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -288,8 +288,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.15); - move_with_ball->setParameter("ball_stabilizing_time", 2.); + move_with_ball->setParameter("dribble_power", 0.25); + move_with_ball->setParameter("ball_stabilizing_time", 1.); move_with_ball->setParameter("reach_threshold", 0.2); } From 756bb7980dff2c4b7b02a98eb223fa2657471a11 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:12:50 +0900 Subject: [PATCH 10/16] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=AE=E9=80=9F=E5=BA=A6=E3=82=92=E4=B8=8B=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 4c20247b0..3827e2fd9 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -263,7 +263,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); command.disableBallAvoidance(); - command.setMaxVelocity(0.5); + command.setMaxVelocity(0.2); command.setMaxAcceleration(1.0); Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); From cd8d09dda246836b5a5207cec63c7806b4836787 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:13:19 +0900 Subject: [PATCH 11/16] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=8C=E6=97=A9=E3=81=99=E3=81=8E=E3=82=8B=E3=81=AE=E3=81=A7?= =?UTF-8?q?=E9=81=85=E3=81=8F=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 3827e2fd9..19396202b 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -297,8 +297,9 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(2.0); - command.setMaxAcceleration(0.5); + command.setMaxVelocity(1.2); + command.setMaxAcceleration(1.0); + command.setOmegaLimit(0.2); return Status::RUNNING; }); From b64fdb5222b767a29ffb7572e56e7081fb08197b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 14:31:00 +0900 Subject: [PATCH 12/16] =?UTF-8?q?rclcpp::Time=E3=82=92RCL=5FROS=5FTIME?= =?UTF-8?q?=E3=82=92=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/visualization_data_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp index 92b7601c7..2af2f0201 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -169,7 +169,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar .strokeWidth(20); visualizer_tracked->add(line_builder.getSvgString()); - auto now = rclcpp::Clock().now(); + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); const double corner_angle = std::acos(0.055 / 0.085); for (const auto & robot : world_model->ours.getAvailableRobots()) { SvgRobotBuilder builder; @@ -192,7 +192,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar // ボールセンサ if ( now.get_clock_type() == robot->ball_sensor_stamp.get_clock_type() && - (now - robot->ball_sensor_stamp).seconds() < 1.0 && robot->ball_sensor) { + std::abs((now - robot->ball_sensor_stamp).seconds()) < 0.01 && robot->ball_sensor) { SvgLineBuilder ball_sensor_line; ball_sensor_line .start(robot->kicker_center() + getVerticalVec(getNormVec(robot->pose.theta)) * 0.055) From c2a9b9ddc039c04fe3070e20b21fb418e7897509 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 14:34:09 +0900 Subject: [PATCH 13/16] =?UTF-8?q?SingleBallPlacementStates::MOVE=5FTO=5FTA?= =?UTF-8?q?RGET=E3=81=A7=E3=81=AE=E8=A7=92=E9=80=9F=E5=BA=A6=E5=88=B6?= =?UTF-8?q?=E9=99=90=E3=81=8C=E5=BC=B7=E3=81=99=E3=81=8E=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 19396202b..5c476501e 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -299,7 +299,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disableRuleAreaAvoidance(); command.setMaxVelocity(1.2); command.setMaxAcceleration(1.0); - command.setOmegaLimit(0.2); + command.setOmegaLimit(1.0); return Status::RUNNING; }); From 1333223e269b10bc9cfefc2eff7a349845210176 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 15:03:31 +0900 Subject: [PATCH 14/16] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=AE=E7=B5=82=E4=BA=86=E6=9D=A1=E4=BB=B6=E3=81=AB=E3=83=9C?= =?UTF-8?q?=E3=83=BC=E3=83=AB=E3=82=BB=E3=83=B3=E3=82=B5=E3=82=92=E7=B5=84?= =?UTF-8?q?=E3=81=BF=E8=BE=BC=E3=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 5c476501e..4c6229b49 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -274,8 +274,26 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, - [this]() { return robot()->getDistance(world_model()->ball.pos) < 0.15; }); + SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); + static int count = 0; + if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) { + if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) { + if (++count > 10) { + count = 0; + return true; + } else { + return false; + } + } else { + count = 0; + return false; + } + } else { + // ボールセンサが動いていないとき + return robot()->getDistance(world_model()->ball.pos) < 0.15; + } + }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { SvgTextBuilder text_builder; From e087fb10a56c5028818a603f75fed4def96e713d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 20:05:10 +0900 Subject: [PATCH 15/16] =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 4c6229b49..46c78692a 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -279,7 +279,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba static int count = 0; if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) { if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) { - if (++count > 10) { + if (++count > 2) { count = 0; return true; } else { From 85e81890e67aa1615097c283378582776c0c6cd0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 01:15:18 +0900 Subject: [PATCH 16/16] =?UTF-8?q?GetBallContact=E3=82=B9=E3=82=AD=E3=83=AB?= =?UTF-8?q?=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/get_ball_contact.hpp | 38 ---------- .../single_ball_placement.hpp | 3 - .../include/crane_robot_skills/skills.hpp | 1 - crane_robot_skills/src/get_ball_contact.cpp | 72 ------------------- .../src/single_ball_placement.cpp | 16 +---- crane_simple_ai/src/crane_commander.cpp | 1 - .../src/simple_ai_planner.cpp | 1 - 7 files changed, 3 insertions(+), 129 deletions(-) delete mode 100644 crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp delete mode 100644 crane_robot_skills/src/get_ball_contact.cpp diff --git a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp deleted file mode 100644 index a7b209dd3..000000000 --- a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// Copyright (c) 2023 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#ifndef CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ -#define CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ - -#include -#include -#include - -namespace crane::skills -{ -class GetBallContact : public SkillBase -{ -public: - explicit GetBallContact(RobotCommandWrapperBase::SharedPtr & base); - - Status update() override; - - void print(std::ostream & out) const override; - -private: - Vector2 getApproachNormVec(); - - std::optional last_contact_start_time; - - builtin_interfaces::msg::Time last_contact_time; - - Point & last_contact_point; - - // double target_distance = 0.0; -}; - -} // namespace crane::skills -#endif // CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp index aa7ea90e0..e3f839371 100644 --- a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp +++ b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp @@ -11,7 +11,6 @@ #include #include -#include "get_ball_contact.hpp" #include "go_over_ball.hpp" #include "move_with_ball.hpp" #include "robot_command_as_skill.hpp" @@ -38,8 +37,6 @@ class SingleBallPlacement private: std::shared_ptr go_over_ball; - std::shared_ptr get_ball_contact; - std::shared_ptr move_with_ball; std::shared_ptr sleep = nullptr; diff --git a/crane_robot_skills/include/crane_robot_skills/skills.hpp b/crane_robot_skills/include/crane_robot_skills/skills.hpp index 50885d1ad..26b0b5d53 100644 --- a/crane_robot_skills/include/crane_robot_skills/skills.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skills.hpp @@ -11,7 +11,6 @@ #include #include #include -#include #include #include #include diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp deleted file mode 100644 index 5e554909b..000000000 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// Copyright (c) 2024 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#include - -namespace crane::skills -{ -GetBallContact::GetBallContact(RobotCommandWrapperBase::SharedPtr & base) -: SkillBase("GetBallContact", base), - last_contact_point(getContextReference("last_contact_point")) -{ - setParameter("min_contact_duration", 0.5); - setParameter("dribble_power", 0.5); -} - -Status GetBallContact::update() -{ - if ( - robot()->ball_contact.getContactDuration() > - std::chrono::duration(getParameter("min_contact_duration"))) { - return Status::SUCCESS; - } else { - auto approach_vec = getApproachNormVec(); - command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); - command.setDribblerTargetPosition(world_model()->ball.pos + approach_vec * 0.05); - command.dribble(getParameter("dribble_power")); - command.disableBallAvoidance(); - return Status::RUNNING; - } -} - -void GetBallContact::print(std::ostream & out) const -{ - out << "[GetBallContact] "; - auto contact_duration = std::chrono::duration_cast( - robot()->ball_contact.getContactDuration()) - .count(); - if (contact_duration > 0) { - out << "contacted: " << contact_duration << "ms"; - } else { - out << "ball distance: " << (robot()->pose.pos - world_model()->ball.pos).norm(); - } -} - -Vector2 GetBallContact::getApproachNormVec() -{ - // if robot is far away from ball, the approach angle is the angle to the ball from robot - // if robot is close to ball, the approach angle is robot angle - // and, the approach angle is interpolated between these two cases - constexpr double FAR_THRESHOLD = 3.5; - constexpr double NEAR_THRESHOLD = 0.5; - - Vector2 far_vec{(robot()->pose.pos - world_model()->ball.pos).normalized()}; - Vector2 near_vec{cos(robot()->pose.theta), sin(robot()->pose.theta)}; - - double distance = (robot()->pose.pos - world_model()->ball.pos).norm(); - - return [&]() { - if (distance > FAR_THRESHOLD) { - return far_vec; - } else if (distance < NEAR_THRESHOLD) { - return near_vec; - } else { - double ratio = (distance - NEAR_THRESHOLD) / (FAR_THRESHOLD - NEAR_THRESHOLD); - return (far_vec * ratio + near_vec * (1.0 - ratio)).normalized(); - } - }(); -} -} // namespace crane::skills diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 46c78692a..f4611233c 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -156,13 +156,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // 失敗の場合は最初に戻る addTransition( SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH, - SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() { - if (not get_ball_contact) { - return false; - } else { - return skill_status == Status::FAILURE; - } - }); + SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, + [this]() { return skill_status == Status::FAILURE; }); // PULL_BACK_FROM_EDGE_PULL addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, [this]() { @@ -256,11 +251,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba .fill("white") .fontSize(100); visualizer->add(text_builder.getSvgString()); - // if (not get_ball_contact) { - // get_ball_contact = std::make_shared(command_base); - // } - // - // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); command.disableBallAvoidance(); command.setMaxVelocity(0.2); @@ -410,7 +400,7 @@ void SingleBallPlacement::print(std::ostream & os) const go_over_ball->print(os); break; case CONTACT_BALL: - get_ball_contact->print(os); + os << " CONTACT_BALL"; break; case MOVE_TO_TARGET: move_with_ball->print(os); diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp index 79b3d1d48..083ea25ad 100644 --- a/crane_simple_ai/src/crane_commander.cpp +++ b/crane_simple_ai/src/crane_commander.cpp @@ -70,7 +70,6 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); - setUpSkillDictionary(); // setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); diff --git a/session/crane_planner_plugins/src/simple_ai_planner.cpp b/session/crane_planner_plugins/src/simple_ai_planner.cpp index ed97d73b8..7bee0d6b4 100644 --- a/session/crane_planner_plugins/src/simple_ai_planner.cpp +++ b/session/crane_planner_plugins/src/simple_ai_planner.cpp @@ -43,7 +43,6 @@ SimpleAIPlanner::SimpleAIPlanner(WorldModelWrapper::SharedPtr & world_model, rcl setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); - setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary();