diff --git a/common/cpp/robocin/geometry/point2d.h b/common/cpp/robocin/geometry/point2d.h index decf862a..eca30a08 100644 --- a/common/cpp/robocin/geometry/point2d.h +++ b/common/cpp/robocin/geometry/point2d.h @@ -161,7 +161,7 @@ struct Point2D { inline constexpr Point2D rotatedCounterClockWise(value_type t) const { Point2D result{*this}; - result.rotateCCW(t); + result.rotateCounterClockWise(t); return result; } diff --git a/navigation-ms/navigation-luffy/navigation/parameters/parameters.h b/navigation-ms/navigation-luffy/navigation/parameters/parameters.h index 80015304..a59383a9 100644 --- a/navigation-ms/navigation-luffy/navigation/parameters/parameters.h +++ b/navigation-ms/navigation-luffy/navigation/parameters/parameters.h @@ -1,14 +1,85 @@ #ifndef NAVIGATION_PARAMETERS_PARAMETERS_H #define NAVIGATION_PARAMETERS_PARAMETERS_H +#include #include namespace navigation { // NOLINTBEGIN(*comment*) - constinit const auto pNavigationPollerTimeoutMs = ::robocin::parameters::View<1>::asInt32(10 /*ms ~= 100Hz*/); +inline const static double M_to_MM = 1e3; +inline const static bool USING_GLOBAL_VELOCITY = false; + +inline const static double ANGLE_KP = 2.85; // 0 - 10 + +inline const static double ROTATE_IN_POINT_MIN_VEL_FACTOR = 0.18; // 0 - 1 +inline const static double ROTATE_IN_POINT_APPROACH_KP = 2; // 0 - 10 +inline const static double ROTATE_IN_POINT_MAX_VELOCITY = 1.8; // 0 - 5 +inline const static double ROTATE_IN_POINT_ANGLE_KP = 5; // 0 - 10 + +inline const static double ROTATE_IN_POINT_DIST_THRESHOLD = 220; +inline const static double ROTATE_IN_POINT_DIST_THRESHOLD_WITH_BALL = 145; +inline const static double ROTATE_IN_POINT_WITH_BALL_ADDITIONAL_DISTANCE = 30; + +inline const static double ROBOT_MAX_ANGULAR_ACCELERATION = 14.4; +inline const static double ROBOT_MAX_ANGULAR_VELOCITY = 24.0; +inline const static double ROBOT_MIN_POSSIBLE_LINEAR_VELOCITY = 0.13; + +inline const static double ANGLE_Y_MOVE_EXCLUSION_ZONE = std::numbers::pi / 4; +inline const static double DIST_TO_ADJUST_EXCLUSIVE_ZONE_MM = 400; + +// larc 2023 parameters +inline const static double FACTOR_TO_MULTIPLY_BOUNDARY_WIDTH_WHEN_HITTING_WALLS = 0.6; + +inline const static double CYCLE_STEP = 0.16; +inline const static double ROBOT_VEL_BREAK_DECAY_FACTOR = 3.1; +inline const static double ROBOT_VEL_FAVORABLE_DECAY_FACTOR = 0.003; + +inline static constexpr double DEFAULT_TOLERANCE_TO_DESIRED_POSITION_M = 0.02; +inline static constexpr double SMALL_TOLERANCE_TO_DESIRED_POSITION_M = 0.01; +namespace moving_profile { + +inline const static double STOP_MAX_VELOCITY = 1.350; // m/s + +inline const static double ALLY_MAX_SPEED = 2.500; // m/s +inline const static double ALLY_MIN_SPEED = 0.200; // m/s +inline const static double MIN_DIST_TO_PROP_VELOCITY = 1.100; // m/s + +inline const static double ALLY_MEDIAN_SPEED = 1.200; // m/s +inline const static double ALLY_SLOW_SPEED = 0.250; // m/s + +inline const static double ROBOT_HIGH_PRECISION_LINEAR_ACCELERATION = 1.100; // m/s^2 +inline const static double ROBOT_DEFAULT_LINEAR_ACCELERATION = 4.500; // m/s^2 +inline const static double MAX_SPEED_IN_CARRY_BALL = 0.27; // m/s + +inline const static double GOALKEEPER_MAX_SPEED = 3.400; // m/s +inline const static double ROBOT_LATERAL_CRITICAL_ACCELERATION = 5.400; + +inline const static double APPROACH_BALL_MAX_SPEED = 0.400; // m/s +inline const static double APPROACH_BALL_MIN_SPEED = 0.150; // m/s +inline const static double APPROACH_BALL_PROP_DISTANCE = 1.000; // m/s + +inline const static double KICK_BALL_MAX_SPEED = 0.340; // m/s +inline const static double KICK_BALL_MAX_SPEED_IN_DIRECT_KICK = 0.47; // m/s +inline const static double KICK_BALL_MIN_SPEED = 0.150; // m/s +inline const static double KICK_BALL_MIN_SPEED_IN_DIRECT_KICK = 0.160; // m/s + +inline const static double TOTOZINHO_MAX_SPEED = 1.5; // m/s +inline const static double TOTOZINHO_MIN_SPEED = 1; // m/s + +inline const static double KICK_BALL_SAFE_PROP_DISTANCE = 0.500; // m/s +inline const static double KICK_BALL_BALANCED_PROP_DISTANCE = 0.600; // m/s + +inline const static double KICK_BALL_WITH_ENEMY_MAX_SPEED = 2.0; // m/s +inline const static double KICK_BALL_WITH_ENEMY_MIN_SPEED = 1.20; // m/s + +inline const static double GOTO_BALL_WITH_ENEMY_MAX_SPEED = 3.0; // m/s +inline const static double GOTO_BALL_WITH_ENEMY_ACCELERATION = 5.0; // m/s] + +} // namespace moving_profile + // NOLINTEND(*comment*) } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt b/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt index 0470ec1b..507edbb2 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt +++ b/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt @@ -2,6 +2,6 @@ robocin_cpp_library( NAME processing HDRS navigation_processor.h SRCS navigation_processor.cpp - DEPS navigation_utils + DEPS navigation_entities motion_parser ) diff --git a/navigation-ms/navigation-luffy/navigation/processing/utils/CMakeLists.txt b/navigation-ms/navigation-luffy/navigation/processing/entities/CMakeLists.txt similarity index 81% rename from navigation-ms/navigation-luffy/navigation/processing/utils/CMakeLists.txt rename to navigation-ms/navigation-luffy/navigation/processing/entities/CMakeLists.txt index 7d1094e9..d060883e 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/utils/CMakeLists.txt +++ b/navigation-ms/navigation-luffy/navigation/processing/entities/CMakeLists.txt @@ -1,5 +1,5 @@ robocin_cpp_library( - NAME navigation_utils + NAME navigation_entities HDRS robot_move.h SRCS robot_move.cpp DEPS common::geometry diff --git a/navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.cpp b/navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.cpp new file mode 100644 index 00000000..49d03e30 --- /dev/null +++ b/navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.cpp @@ -0,0 +1 @@ +#include diff --git a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h b/navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.h similarity index 50% rename from navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h rename to navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.h index d993ae65..42c39b98 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h +++ b/navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.h @@ -6,22 +6,22 @@ namespace navigation { class RobotMove { - ::robocin::Point2Df velocity_; + ::robocin::Point2Dd velocity_; double angularVelocity_; public: - RobotMove(::robocin::Point2Df velocity, double angularVelocity) : + RobotMove(::robocin::Point2Dd velocity, double angularVelocity) : velocity_(velocity), angularVelocity_(angularVelocity) {} - RobotMove(double angularVelocity) : - velocity_(::robocin::Point2Df(0.0, 0.0)), + explicit RobotMove(double angularVelocity) : + velocity_(::robocin::Point2Dd(0.0, 0.0)), angularVelocity_(angularVelocity) {} - RobotMove() : velocity_(::robocin::Point2Df(0.0, 0.0)), angularVelocity_(0.0) {} + RobotMove() : velocity_(::robocin::Point2Dd(0.0, 0.0)), angularVelocity_(0.0) {} - ::robocin::Point2Df velocity() const { return velocity_; } - double angularVelocity() const { return angularVelocity_; } + [[nodiscard]] ::robocin::Point2Dd velocity() const { return velocity_; } + [[nodiscard]] double angularVelocity() const { return angularVelocity_; } }; } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/CMakeLists.txt b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/CMakeLists.txt index bb7852bb..9a15b409 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/CMakeLists.txt +++ b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/CMakeLists.txt @@ -3,5 +3,7 @@ robocin_cpp_library( HDRS motion_parser.h SRCS motion_parser.cpp DEPS protocols::protobufs + navigation_entities + angular navigation_utils ) diff --git a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp index 4c94f039..20e1e644 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp +++ b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp @@ -1,13 +1,31 @@ #include "navigation/processing/motion_parser/motion_parser.h" -#include "robocin/geometry/point2d.h" +#include +#include +#include +#include +#include +#include namespace navigation { +namespace { +namespace rc { +using ::protocols::behavior::GoToPoint; +using ::protocols::behavior::PathNode; +using ::protocols::behavior::PrecisionToTarget; +using ::protocols::behavior::RotateInPoint; +using ::protocols::behavior::RotateOnSelf; +using ::protocols::common::Point2Df; +using ::protocols::perception::Robot; +} // namespace rc + +} // namespace + MotionParser::MotionParser() {} -RobotMove MotionParser::fromGoToPoint(const ::protocols::behavior::GoToPoint& go_to_point, - const ::protocols::perception::Robot& robot) { +RobotMove MotionParser::fromGoToPoint(const rc::GoToPoint& go_to_point, + const rc::Robot& robot) { robocin::Point2D robot_position = robocin::Point2D(robot.position().x(), robot.position().y()); robocin::Point2D target_position @@ -46,16 +64,38 @@ RobotMove MotionParser::fromGoToPoint(const ::protocols::behavior::GoToPoint& go } } -RobotMove -MotionParser::fromRotateInPoint(const ::protocols::behavior::RotateInPoint& rotate_in_point, - const ::protocols::perception::Robot& robot) { - - // PROCESSAMENTO DO ROTATEINPOINT - return RobotMove{}; +RobotMove MotionParser::fromRotateInPoint(const rc::RotateInPoint& rotate_in_point, + const rc::Robot& robot) { + + const double velocity = [&]() { + double velocity = rotate_in_point.rotate_velocity(); + velocity *= rotate_in_point.min_velocity(); + + return velocity; + }(); + ::robocin::Point2Dd target = {rotate_in_point.target().x(), rotate_in_point.target().y()}; + ::robocin::Point2Dd robot_pos = {robot.position().x(), robot.position().y()}; + + const double robot_radius = (robot_pos.distanceTo(target)) / M_to_MM; + const double delta_theta = robocin::smallestAngleDiff(robot.angle(), rotate_in_point.target_angle()); + const double approach_kp = rotate_in_point.approach_kp(); + const double angle_kp = rotate_in_point.angle_kp(); + + // Rotate in the smaller angle + const double orientation_factor = rotate_in_point.clockwise() ? 1.0 : -1.0; + const robocin::Point2Dd coordinates + = robocin::Point2Dd(approach_kp * (robot_radius - rotate_in_point.orbit_radius() / M_to_MM), + orientation_factor * velocity); + const robocin::Point2Dd rotated_coordinates = coordinates.rotatedCounterClockWise(robot.angle()); + const double angular_velocity + = ((-(orientation_factor * velocity) / (rotate_in_point.orbit_radius() / M_to_MM)) + + (angle_kp * delta_theta)); + + return RobotMove{rotated_coordinates, angular_velocity}; } -RobotMove MotionParser::fromRotateOnSelf(const ::protocols::behavior::RotateOnSelf& rotate_on_self, - const ::protocols::perception::Robot& robot) { +RobotMove MotionParser::fromRotateOnSelf(const rc::RotateOnSelf& rotate_on_self, + const rc::Robot& robot) { // PROCESSAMENTO DO ROTATEINPOINT return RobotMove{}; diff --git a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h index 73f08778..34b9462c 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h +++ b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h @@ -1,7 +1,7 @@ #ifndef NAVIGATION_PROCESSING_MOTION_PARSER_H #define NAVIGATION_PROCESSING_MOTION_PARSER_H -#include +#include #include #include #include diff --git a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp b/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp deleted file mode 100644 index fc7f1c61..00000000 --- a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp +++ /dev/null @@ -1 +0,0 @@ -#include