diff --git a/docs/source/tutorial.rst b/docs/source/tutorial.rst index b44e331c40e..d836f27c95e 100644 --- a/docs/source/tutorial.rst +++ b/docs/source/tutorial.rst @@ -351,6 +351,7 @@ but here are some more hints. * The motion command for driving in a straight line is :cpp:`"path_target"`. * You will probably need to override some methods relating to passing, but you can leave their implementations empty. They don't need to do anything in your position, as your robot will not pass the ball * The simulator tells you the coordinates of your cursor—these are the same coordinates you can use in your motion commands. +* You will need to add the new file name you create to ``soccer/src/soccer/CMakeLists.txt``. See how this is done for other positions. Testing ~~~~~~~ @@ -668,4 +669,4 @@ Here are all the external links from this document, copied again for your easy r * `C++ Inheritance`_ -.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/ \ No newline at end of file +.. _C++ Inheritance: https://www.learncpp.com/cpp-tutorial/basic-inheritance-in-c/ diff --git a/install/env.sh b/install/env.sh new file mode 100644 index 00000000000..e9bc1810806 --- /dev/null +++ b/install/env.sh @@ -0,0 +1,5 @@ +# Here are some useful environment variables for setting up a consistent ROS environment +# some information on this can be found here: https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html +export ROS_DOMAIN_ID=19 # this is a random number, it's just important to keep it consistent across shells +export ROS_LOCALHOST_ONLY=1 # for our stack specifically, this is quite helpful. + diff --git a/install/setup.bash b/install/setup.bash index b84bbe02bab..6e2b3722020 100755 --- a/install/setup.bash +++ b/install/setup.bash @@ -34,7 +34,7 @@ _pythonpath_add() { # No direct replacement for the command exists, and in Ubuntu 22.04, # this script will produce a warning about this. We may need to find a # replacement library in the future. -_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))") +_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))") _pythonpath_add "${_PYTHON_LIB_PATH}" unset _PYTHON_LIB_PATH diff --git a/install/setup.zsh b/install/setup.zsh index 4d6b7f16a21..db45a72c353 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -30,7 +30,7 @@ _pythonpath_add() { fi } -_PYTHON_LIB_PATH=$(python3 -c "from distutils.sysconfig import get_python_lib; print(get_python_lib(prefix='${_INSTALL_PATH}'))") +_PYTHON_LIB_PATH=$(python3 -c "import sysconfig; print(sysconfig.get_path('purelib'))") _pythonpath_add "${_PYTHON_LIB_PATH}" unset _PYTHON_LIB_PATH diff --git a/launch/framework.bash b/launch/framework.bash index bf67387d40a..b5dcc9e9f77 100755 --- a/launch/framework.bash +++ b/launch/framework.bash @@ -10,7 +10,7 @@ if [ -z "$binary" ]; then fi # Run the binary in the background -"$binary" & +"$binary" -g 2020B --realism RC2021 & binary_pid=$! # Ensure that pressing Ctrl+C kills all subprocesses diff --git a/makefile b/makefile index 4ddb5959fb1..837c56b2949 100644 --- a/makefile +++ b/makefile @@ -72,11 +72,11 @@ run-sim: # run our stack with default flags # TODO: actually name our software stack something run-our-stack: - ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True + ros2 launch rj_robocup soccer.launch.py run_sim:=True # run sim with external referee (SSL Game Controller) run-sim-external: - ROS_LOCALHOST_ONLY=1 ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False + ros2 launch rj_robocup soccer.launch.py run_sim:=True use_internal_ref:=False run-sim-ex: run-sim-external @@ -95,7 +95,7 @@ run-manual: # same as run-real, with different server port run-alt-real: - ROS_DOMAIN_ID=2 ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b + ros2 launch rj_robocup soccer.launch.py run_sim:=False use_sim_radio:=False server_port:=25564 use_internal_ref:=False team_name:=AltRoboJackets team_flag:=-b # run sim2play (requires external referee) run-sim2play: diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 452c350b9e2..2e776364e2a 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -40,6 +40,7 @@ set(ROBOCUP_LIB_SRC planning/planner/path_target_path_planner.cpp planning/planner/pivot_path_planner.cpp planning/planner/line_pivot_path_planner.cpp + planning/planner/rotate_path_planner.cpp planning/planner/plan_request.cpp planning/planner/settle_path_planner.cpp planning/planner/goalie_idle_path_planner.cpp diff --git a/soccer/src/soccer/control/trapezoidal_motion.cpp b/soccer/src/soccer/control/trapezoidal_motion.cpp index a0aee1a2044..c4aded7d6f1 100644 --- a/soccer/src/soccer/control/trapezoidal_motion.cpp +++ b/soccer/src/soccer/control/trapezoidal_motion.cpp @@ -112,9 +112,9 @@ double Trapezoidal::get_time(double distance, double path_length, double max_spe } } -bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap, - double start_speed, double final_speed, double& pos_out, - double& speed_out) { +bool Trapezoidal::trapezoidal_motion(double path_length, double max_speed, double max_acc, + double time_into_lap, double start_speed, double final_speed, + double& pos_out, double& speed_out) { // begin by assuming that there's enough time to get up to full speed // we do this by calculating the full ramp-up and ramp-down, then seeing // if the distance travelled is too great. If it's gone too far, this is diff --git a/soccer/src/soccer/control/trapezoidal_motion.hpp b/soccer/src/soccer/control/trapezoidal_motion.hpp index 742583a9762..9ac1091bdc3 100644 --- a/soccer/src/soccer/control/trapezoidal_motion.hpp +++ b/soccer/src/soccer/control/trapezoidal_motion.hpp @@ -1,23 +1,5 @@ #pragma once -/** - * This function evaluates a control situation using "bang-bang". The robot - * accelerates at maxAcceleration until it hits max speed, then decelerates at - * max acc to the end point. - * - * @param[in] pathLength The total distance we are trying to travel - * @param[in] timeIntoLap How long since we started moving along the trapezoid - * @param[in] finalSpeed The speed we'd like to be going at the end of the - * trapezoid - * @param[out] distOut The distance to be at at the given time - * @param[out] speedOut The speed to be at at the given time - * @return true if the trapezoid is valid at the given time, false - * otherwise - */ -bool trapezoidal_motion(double path_length, double max_speed, double max_acc, - double time_into_lap, double start_speed, double final_speed, - double& pos_out, double& speed_out); - namespace Trapezoidal { /** * Estimates how long it would take to move to a certain distance down a path @@ -37,4 +19,21 @@ namespace Trapezoidal { double get_time(double distance, double path_length, double max_speed, double max_acc, double start_speed, double final_speed); +/** + * This function evaluates a control situation using "bang-bang". The robot + * accelerates at maxAcceleration until it hits max speed, then decelerates at + * max acc to the end point. + * + * @param[in] pathLength The total distance we are trying to travel + * @param[in] timeIntoLap How long since we started moving along the trapezoid + * @param[in] finalSpeed The speed we'd like to be going at the end of the + * trapezoid + * @param[out] distOut The distance to be at at the given time + * @param[out] speedOut The speed to be at at the given time + * @return true if the trapezoid is valid at the given time, false + * otherwise + */ +bool trapezoidal_motion(double path_length, double max_speed, double max_acc, double time_into_lap, + double start_speed, double final_speed, double& pos_out, double& speed_out); + } // namespace Trapezoidal diff --git a/soccer/src/soccer/control/trapezoidal_motion_test.cpp b/soccer/src/soccer/control/trapezoidal_motion_test.cpp index f99a0fc70c3..f9d10d8dc74 100644 --- a/soccer/src/soccer/control/trapezoidal_motion_test.cpp +++ b/soccer/src/soccer/control/trapezoidal_motion_test.cpp @@ -16,8 +16,8 @@ bool trapezoid1(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { auto time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -34,8 +34,8 @@ bool trapezoid2(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -55,8 +55,8 @@ bool triangle1(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -76,8 +76,8 @@ bool triangle2(double t, double& pos_out, double& speed_out) { double start_speed = 0; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -97,8 +97,8 @@ bool triangle3(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 1; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, @@ -118,8 +118,8 @@ bool triangle4(double t, double& pos_out, double& speed_out) { double start_speed = 1; double final_speed = 0; - bool valid = trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, final_speed, - pos_out, speed_out); + bool valid = Trapezoidal::trapezoidal_motion(path_length, max_speed, max_acc, t, start_speed, + final_speed, pos_out, speed_out); if (valid) { double time = Trapezoidal::get_time(pos_out, path_length, max_speed, max_acc, start_speed, diff --git a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp index a6b7fdd0dcd..b68e14adaf8 100644 --- a/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/line_pivot_path_planner.cpp @@ -51,6 +51,7 @@ LinePivotPathPlanner::State LinePivotPathPlanner::next_state(const PlanRequest& double vel = plan_request.world_state->get_robot(true, static_cast(plan_request.shell_id)) .velocity.linear() .mag(); + if (current_state_ == LINE && (target_point.dist_to(current_point) < 0.3) && (vel < 0.3) && (!plan_request.play_state.is_stop())) { return PIVOT; diff --git a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp index 995acbbfc1b..1a4e7b6e7c4 100644 --- a/soccer/src/soccer/planning/planner/path_target_path_planner.cpp +++ b/soccer/src/soccer/planning/planner/path_target_path_planner.cpp @@ -87,6 +87,10 @@ AngleFunction PathTargetPathPlanner::get_angle_function(const PlanRequest& reque return AngleFns::face_angle(std::get(face_option).target); } + if (std::holds_alternative(face_option)) { + return AngleFns::face_point(request.motion_command.target.position); + } + // default to facing tangent to path // (rj_convert in motion_command.hpp also follows this default) return AngleFns::tangent; diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.cpp b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp new file mode 100644 index 00000000000..93c2220bbbb --- /dev/null +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.cpp @@ -0,0 +1,80 @@ +#include "rotate_path_planner.hpp" + +#include +#include + +#include +#include +#include + +#include "planning/instant.hpp" +#include "planning/planning_params.hpp" +#include "planning/primitives/angle_planning.hpp" +#include "planning/primitives/path_smoothing.hpp" +#include "planning/primitives/trapezoidal_motion.hpp" +#include "planning/primitives/velocity_profiling.hpp" +#include "planning/trajectory.hpp" + +namespace planning { +using namespace rj_geometry; + +Trajectory RotatePathPlanner::plan(const PlanRequest& request) { + return pivot(request); // type is Trajectory +} + +bool RotatePathPlanner::is_done() const { + if (!cached_angle_change_) { + return false; + } + return abs(cached_angle_change_.value()) < + degrees_to_radians(static_cast(kIsDoneAngleChangeThresh)); +} + +Trajectory RotatePathPlanner::pivot(const PlanRequest& request) { + const RobotInstant& start_instant = request.start; + const auto& linear_constraints = request.constraints.mot; + const auto& rotation_constraints = request.constraints.rot; + + rj_geometry::ShapeSet static_obstacles; + std::vector dynamic_obstacles; + fill_obstacles(request, &static_obstacles, &dynamic_obstacles, false); + + const MotionCommand& command = request.motion_command; + + auto pivot_point = + request.world_state->get_robot(true, static_cast(request.shell_id)).pose.position(); + auto pivot_target = command.target.position; + + double start_angle = + request.world_state->get_robot(true, static_cast(request.shell_id)).pose.heading(); + + double target_angle = pivot_point.angle_to(pivot_target); + double angle_change = fix_angle_radians(target_angle - start_angle); + + cached_angle_change_ = angle_change; + + Trajectory path{}; + + if (abs(*cached_target_angle_ - target_angle) < degrees_to_radians(kIsDoneAngleChangeThresh)) { + if (cached_path_) { + path = cached_path_.value(); + } else { + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), + request.constraints.rot); + path.stamp(RJ::now()); + cached_path_ = path; + } + } else { + cached_path_.reset(); + plan_angles(&path, start_instant, AngleFns::face_point(pivot_target), + request.constraints.rot); + path.stamp(RJ::now()); + cached_path_ = path; + } + + cached_target_angle_ = target_angle; + + return path; +} + +} // namespace planning diff --git a/soccer/src/soccer/planning/planner/rotate_path_planner.hpp b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp new file mode 100644 index 00000000000..7b2e5c86ad5 --- /dev/null +++ b/soccer/src/soccer/planning/planner/rotate_path_planner.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "path_planner.hpp" +#include "path_target_path_planner.hpp" + +namespace planning { +/** + * Path planner that only rotates the robot about a point given. + * + * Params taken from MotionCommand: + * target.position - robot will face this point when done + * target.pivot_point - robot will pivot around this point + */ +class RotatePathPlanner : public PathPlanner { +public: + RotatePathPlanner() : PathPlanner("rotate") {} + ~RotatePathPlanner() override = default; + + RotatePathPlanner(RotatePathPlanner&&) noexcept = default; + RotatePathPlanner& operator=(RotatePathPlanner&&) noexcept = default; + RotatePathPlanner(const RotatePathPlanner&) = default; + RotatePathPlanner& operator=(const RotatePathPlanner&) = default; + + Trajectory plan(const PlanRequest& request) override; + + void reset() override { + cached_target_angle_ = std::nullopt; + cached_angle_change_ = std::nullopt; + } + [[nodiscard]] bool is_done() const override; + +private: + Trajectory previous_; + + std::optional cached_target_angle_; // equivalent to previously recorded accorded + std::optional cached_angle_change_; + + std::optional cached_path_; + + PathTargetPathPlanner path_target_{}; + + Trajectory pivot(const PlanRequest& request); + + static constexpr double kIsDoneAngleChangeThresh{1.0}; +}; +} // namespace planning \ No newline at end of file diff --git a/soccer/src/soccer/planning/planner_for_robot.cpp b/soccer/src/soccer/planning/planner_for_robot.cpp index 6a6cfc22042..7a302773c2f 100644 --- a/soccer/src/soccer/planning/planner_for_robot.cpp +++ b/soccer/src/soccer/planning/planner_for_robot.cpp @@ -9,6 +9,7 @@ #include "planning/planner/line_pivot_path_planner.hpp" #include "planning/planner/path_target_path_planner.hpp" #include "planning/planner/pivot_path_planner.hpp" +#include "planning/planner/rotate_path_planner.hpp" #include "planning/planner/settle_path_planner.hpp" namespace planning { @@ -32,6 +33,7 @@ PlannerForRobot::PlannerForRobot(int robot_id, rclcpp::Node* node, path_planners_[LineKickPathPlanner().name()] = std::make_unique(); path_planners_[PivotPathPlanner().name()] = std::make_unique(); path_planners_[LinePivotPathPlanner().name()] = std::make_unique(); + path_planners_[RotatePathPlanner().name()] = std::make_unique(); path_planners_[EscapeObstaclesPathPlanner().name()] = std::make_unique(); diff --git a/soccer/src/soccer/planning/primitives/angle_planning.cpp b/soccer/src/soccer/planning/primitives/angle_planning.cpp index 2b38ff6d1d8..82adb2b338b 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.cpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.cpp @@ -5,12 +5,40 @@ namespace planning { void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, - const AngleFunction& angle_function, - const RotationConstraints& /* constraints */) { + const AngleFunction& angle_function, const RotationConstraints& constraints) { const RJ::Time start_time = start_instant.stamp; if (trajectory->empty()) { - throw std::invalid_argument("Cannot profile angles for empty trajectory."); + trajectory->append_instant(start_instant); + double delta_angle = + angle_function(start_instant.linear_motion(), start_instant.heading(), nullptr) - + start_instant.heading(); + + if (delta_angle > M_PI) { + delta_angle -= 2 * M_PI; + } + + if (delta_angle < -M_PI) { + delta_angle += 2 * M_PI; + } + + double time = Trapezoidal::get_time(abs(delta_angle), abs(delta_angle), + constraints.max_speed, constraints.max_accel, 0, 0); + + for (int i = 1; i < time / TIME_STEP; i++) { + double pos_out = 0; + double speed_out = 0; + Trapezoidal::trapezoidal_motion(abs(delta_angle), constraints.max_speed, + constraints.max_accel, i * TIME_STEP, 0, 0, pos_out, + speed_out); + pos_out *= std::signbit(delta_angle) ? -1 : 1; + trajectory->append_instant(RobotInstant{ + rj_geometry::Pose{start_instant.position(), pos_out + start_instant.heading()}, + rj_geometry::Twist{start_instant.linear_velocity(), speed_out}, + start_instant.stamp + RJ::Seconds(i * TIME_STEP)}); + } + trajectory->mark_angles_valid(); + return; } if (!trajectory->check_time(start_time)) { diff --git a/soccer/src/soccer/planning/primitives/angle_planning.hpp b/soccer/src/soccer/planning/primitives/angle_planning.hpp index 0a1dd0c40f2..3a39d004f57 100644 --- a/soccer/src/soccer/planning/primitives/angle_planning.hpp +++ b/soccer/src/soccer/planning/primitives/angle_planning.hpp @@ -5,6 +5,7 @@ #include #include +#include "control/trapezoidal_motion.hpp" #include "planning/instant.hpp" #include "planning/robot_constraints.hpp" #include "planning/trajectory.hpp" @@ -114,4 +115,5 @@ void plan_angles(Trajectory* trajectory, const RobotInstant& start_instant, const AngleFunction& angle, const RotationConstraints& constraints); +constexpr double TIME_STEP = 0.001; } // namespace planning diff --git a/soccer/src/soccer/radio/sim_radio.cpp b/soccer/src/soccer/radio/sim_radio.cpp index 4bde12611f2..c0901018f56 100644 --- a/soccer/src/soccer/radio/sim_radio.cpp +++ b/soccer/src/soccer/radio/sim_radio.cpp @@ -88,7 +88,6 @@ SimRadio::SimRadio(bool blue_team) // file probably an issue with the hacky way I got param files to dynamically load std::string localhost = "127.0.0.1"; this->get_parameter_or("interface", param_radio_interface_, localhost); - SPDLOG_INFO("SimRadio param_radio_interface_ {}", param_radio_interface_); address_ = boost::asio::ip::make_address(param_radio_interface_).to_v4(); robot_control_endpoint_ = ip::udp::endpoint(address_, blue_team_ ? kSimBlueCommandPort : kSimYellowCommandPort); diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp index 3d288c8228b..3c45fd97374 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.cpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.cpp @@ -11,9 +11,9 @@ SoloOffense::SoloOffense(int r_id) : Position{r_id, "SoloOffense"} {} std::optional SoloOffense::derived_get_task(RobotIntent intent) { // Get next state, and if different, reset clock State new_state = next_state(); - if (new_state != current_state_) { - // SPDLOG_INFO("New State: {}", std::to_string(static_cast(new_state))); - } + // if (new_state != current_state_) { + // } + SPDLOG_INFO("New State: {}", std::to_string(static_cast(new_state))); current_state_ = new_state; // Calculate task based on state @@ -52,14 +52,25 @@ SoloOffense::State SoloOffense::next_state() { } case TO_BALL: { if (check_is_done()) { - target_ = calculate_best_shot(); + return ROTATE; + } + return TO_BALL; + } + case ROTATE: { + if (check_is_done()) { + counter_ = 0; + kick_ = true; return KICK; } + return ROTATE; } case KICK: { - if (check_is_done()) { + if (!kick_ || + (last_world_state_->get_robot(true, robot_id_).pose.position() - current_point) + .mag() > kRobotRadius * 5) { return TO_BALL; } + return KICK; } } return current_state_; @@ -80,21 +91,47 @@ std::optional SoloOffense::state_to_task(RobotIntent intent) { return intent; } case TO_BALL: { - planning::LinearMotionInstant target{field_dimensions_.their_goal_loc()}; - auto pivot_cmd = planning::MotionCommand{"line_pivot", target, planning::FaceTarget{}, - false, last_world_state_->ball.position}; - pivot_cmd.pivot_radius = kRobotRadius * 2.5; + rj_geometry::Point robotToBall = + (last_world_state_->ball.position - + last_world_state_->get_robot(true, robot_id_).pose.position()); + double slowDown = 1.0; + double length = robotToBall.mag() - kRobotRadius * slowDown; + robotToBall = robotToBall.normalized(length); + planning::LinearMotionInstant target{ + last_world_state_->get_robot(true, robot_id_).pose.position() + robotToBall}; + auto pivot_cmd = + planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true}; intent.motion_command = pivot_cmd; + intent.dribbler_speed = 255; + return intent; + } + case ROTATE: { + planning::LinearMotionInstant target{calculate_best_shot()}; + auto pivot_cmd = + planning::MotionCommand{"rotate", target, planning::FaceTarget{}, false}; + intent.motion_command = pivot_cmd; + intent.dribbler_speed = 255; return intent; } case KICK: { - auto line_kick_cmd = - planning::MotionCommand{"line_kick", planning::LinearMotionInstant{target_}}; - - intent.motion_command = line_kick_cmd; + // double scaleFactor = 0.1; + // rj_geometry::Point point = (last_world_state_->ball.position - + // last_world_state_->get_robot(true, + // robot_id_).pose.position()).normalized(scaleFactor); point += + // last_world_state_->get_robot(true, robot_id_).pose.position(); + // planning::LinearMotionInstant target{point}; + planning::LinearMotionInstant target{calculate_best_shot()}; + // planning::LinearMotionInstant target{last_world_state_->ball.position}; + auto kick_cmd = + planning::MotionCommand{"path_target", target, planning::FaceTarget{}, true}; + intent.motion_command = kick_cmd; intent.shoot_mode = RobotIntent::ShootMode::KICK; intent.trigger_mode = RobotIntent::TriggerMode::ON_BREAK_BEAM; intent.kick_speed = 4.0; + counter_++; + if (counter_ > 15) { + kick_ = false; + } return intent; } diff --git a/soccer/src/soccer/strategy/agent/position/solo_offense.hpp b/soccer/src/soccer/strategy/agent/position/solo_offense.hpp index cbbd48b0e49..4f4608dc9e8 100644 --- a/soccer/src/soccer/strategy/agent/position/solo_offense.hpp +++ b/soccer/src/soccer/strategy/agent/position/solo_offense.hpp @@ -36,7 +36,7 @@ class SoloOffense : public Position { */ std::optional derived_get_task(RobotIntent intent) override; - enum State { TO_BALL, KICK, MARKER }; + enum State { TO_BALL, KICK, MARKER, ROTATE }; State current_state_ = TO_BALL; @@ -44,6 +44,9 @@ class SoloOffense : public Position { int marking_id_; + bool kick_ = false; + int counter_ = 0; + /** * @return what the state should be right now. called on each get_task tick */ diff --git a/source.bash b/source.bash index 85a5e0bedf1..d86ad19995f 100755 --- a/source.bash +++ b/source.bash @@ -9,3 +9,6 @@ if [[ $SHELL == *"zsh"* ]]; then source /opt/ros/humble/setup.zsh source install/setup.zsh fi + +source install/env.sh +