diff --git a/crane_local_planner/CMakeLists.txt b/crane_local_planner/CMakeLists.txt index c4d20b10..cf6509d1 100755 --- a/crane_local_planner/CMakeLists.txt +++ b/crane_local_planner/CMakeLists.txt @@ -15,7 +15,6 @@ add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS") # find dependencies find_package(ament_cmake_auto REQUIRED) find_package(rclcpp_components REQUIRED) -# find_package(backward_ros REQUIRED) ament_auto_find_build_dependencies() ament_auto_add_library(${PROJECT_NAME}_component SHARED @@ -40,9 +39,6 @@ ament_auto_add_executable(${PROJECT_NAME}_node src/${PROJECT_NAME}_node.cpp ) -# include(/opt/ros/$ENV{ROS_DISTRO}/share/backward_ros/cmake/BackwardConfig.cmake) -# add_backward(${PROJECT_NAME}_node) - if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/crane_local_planner/package.xml b/crane_local_planner/package.xml index 177c2852..17901fa1 100755 --- a/crane_local_planner/package.xml +++ b/crane_local_planner/package.xml @@ -9,7 +9,6 @@ ament_cmake_auto - closest_point_vendor crane_msg_wrappers crane_msgs diff --git a/crane_robot_skills/include/crane_robot_skills/attacker.hpp b/crane_robot_skills/include/crane_robot_skills/attacker.hpp index 8175dde8..d4e296e2 100644 --- a/crane_robot_skills/include/crane_robot_skills/attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/attacker.hpp @@ -23,7 +23,6 @@ namespace crane::skills enum class AttackerState { ENTRY_POINT, FORCED_PASS, - CUT_THEIR_PASS, STEAL_BALL, REDIRECT_GOAL_KICK, GOAL_KICK, diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 4d674f78..81410c80 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -95,23 +95,6 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return Status::RUNNING; }); - // addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool { - // return not world_model()->isOurBallByBallOwnerCalculator() && - // world_model()->ball.isMoving(0.2) && - // world_model()->ball.isMovingTowards(robot()->pose.pos); - // }); - // - // addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { - // return world_model()->isOurBallByBallOwnerCalculator() or world_model()->ball.isStopped(0.2); - // }); - // - // addStateFunction(AttackerState::CUT_THEIR_PASS, [this]() -> Status { - // visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5); - // receive_skill.setParameter("enable_redirect", false); - // receive_skill.setParameter("policy", std::string("min_slack")); - // return receive_skill.run(); - // }); - addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool { // 止まっているボールを相手が持っているとき const auto enemy_robots = world_model()->theirs.getAvailableRobots(); diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 7b1da70a..bdb81e54 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -5,6 +5,7 @@ // https://opensource.org/licenses/MIT. #include +#include namespace crane::skills { @@ -35,9 +36,17 @@ Status Goalie::update() phase = "ペナルティキック"; inplay(false); break; - default: - inplay(true); + default: { + if ( + world_model()->play_situation.getRefereeCommandID() == + robocup_ssl_msgs::msg::Referee::COMMAND_STOP) { + // STOPのときにはボールを排出しない + inplay(false); + } else { + inplay(true); + } break; + } } visualizer->addPoint(robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "white", 1., phase); diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 95ef7340..80431ff2 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -277,7 +277,6 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame & auto last_ball_info_history = ball_info_history[ball_info_history_size - 1]; double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time; - // double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time; double elapsed_time_since_last_detected = (now() - last_ball_detect_time).seconds(); // 0.5secビジョンから見えていなければ見失った