Add RotateInPoint Motion

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;
   }
 

navigation-ms/navigation-luffy/navigation/parameters/parameters.h

@@ -1,14 +1,85 @@
 #ifndef NAVIGATION_PARAMETERS_PARAMETERS_H
 #define NAVIGATION_PARAMETERS_PARAMETERS_H
 
+#include <numbers>
 #include <robocin/parameters/parameters.h>
 
 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
 

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
 )

navigation-ms/navigation-luffy/navigation/processing/utils/CMakeLists.txt → 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 

navigation-ms/navigation-luffy/navigation/processing/entities/robot_move.cpp

@@ -0,0 +1 @@
+#include <navigation/processing/entities/robot_move.h>

navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h → 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

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
 )

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 <cstdlib>
+#include <navigation/parameters/parameters.h>
+#include <navigation/processing/entities/robot_move.h>
+#include <robocin/geometry/mathematics.h>
+#include <robocin/geometry/point2d.h>
+#include <robocin/utility/angular.h>
 
 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{};

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 <navigation/processing/utils/robot_move.h>
+#include <navigation/processing/entities/robot_move.h>
 #include <protocols/behavior/behavior_unification.pb.h>
 #include <protocols/behavior/motion.pb.h>
 #include <protocols/perception/detection.pb.h>