pushing_with_wsl_vscode

RoboTeamTwente · Nov 14, 2024 · 137f3b7 · 137f3b7
1 parent d56259f
commit 137f3b7
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 13 deletions.
diff --git a/roboteam_ai/src/stp/skills/GoToPos.cpp b/roboteam_ai/src/stp/skills/GoToPos.cpp
@@ -10,7 +10,7 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept {
     auto field = info.getField().value();
     auto avoidObj = info.getObjectsToAvoid();
     auto targetPos = info.getPositionToMoveTo().value();
-    auto targetVel = info.getTargetVelocity();
+   // auto targetVel = info.getTargetVelocity();
     auto roleName = info.getRoleName();
     auto ballLocation = info.getBall()->get()->position;
     RefCommand currentGameState = GameStateManager::getCurrentGameState().getCommandId();
@@ -29,9 +29,10 @@ Status GoToPos::onUpdate(const StpInfo &info) noexcept {
     }
     auto [vel, acc] = info.getCurrentWorld()->getRobotPositionController()->computeAndTrackTrajectory(
         info.getCurrentWorld(), field, robot->getId(), robot->getPos(), robot->getVel(), targetPos, targetVel, info.getMaxRobotVelocity(), info.getMaxJerk(), avoidObj);
-    command.velocity = vel;
-    command.acceleration = acc;
-
+    // command.velocity = vel;
+    // command.acceleration = acc;
+     // Set command position and orientation
+    command.position = targetPos;
     command.yaw = info.getYaw();
 
     command.dribblerOn = info.getDribblerOn();

diff --git a/roboteam_utils/include/roboteam_utils/RobotCommands.hpp b/roboteam_utils/include/roboteam_utils/RobotCommands.hpp
@@ -2,7 +2,7 @@
 
 #include <roboteam_utils/Angle.h>
 #include <roboteam_utils/Vector2.h>
-
+#include <string>
 #include <vector>
 
 namespace rtt {
@@ -19,10 +19,11 @@ typedef struct RobotCommand {
     int id = 0;  // [0,15] The id of robot
 
     // Positioning related variables
-    Vector2 velocity;                    // (m/s) Target velocity of the robot
-    Vector2 acceleration;                // (m/s^2) Target acceleration of the robot
+    // Vector2 velocity;                    // (m/s) Target velocity of the robot
+    // Vector2 acceleration;                // (m/s^2) Target acceleration of the robot
+    Vector2 position;                    // Vector for desired poosition (x,y)
     Angle yaw;                           // (rad) [-PI, PI] The target yaw of the robot
-    double targetAngularVelocity = 0.0;  // (rad/s) The target angular velocity of the robot
+    // double targetAngularVelocity = 0.0;  // (rad/s) The target angular velocity of the robot
     bool useAngularVelocity = 0.0;       // True if angular velocity should be used instead of yaw
 
     Angle cameraYawOfRobot;              // (rad) The current yaw of the robot according to the camera

diff --git a/roboteam_utils/src/utils/RobotCommands.cpp b/roboteam_utils/src/utils/RobotCommands.cpp
@@ -17,19 +17,20 @@ std::string kickTypeToString(KickType type) {
 }
 
 bool RobotCommand::operator==(const RobotCommand &other) const {
-    return this->id == other.id && this->velocity == other.velocity && this->yaw == other.yaw && this->targetAngularVelocity == other.targetAngularVelocity &&
-           this->useAngularVelocity == other.useAngularVelocity && this->cameraYawOfRobot == other.cameraYawOfRobot && this->cameraYawOfRobotIsSet == other.cameraYawOfRobotIsSet &&
+    return this->id == other.id && this->position == other.position && this->yaw == other.yaw  &&
+           this->cameraYawOfRobot == other.cameraYawOfRobot && this->cameraYawOfRobotIsSet == other.cameraYawOfRobotIsSet &&
            this->kickSpeed == other.kickSpeed && this->waitForBall == other.waitForBall && this->kickType == other.kickType && this->kickAtYaw == other.kickAtYaw &&
            this->dribblerOn == other.dribblerOn && this->wheelsOff == other.wheelsOff;
 }
 
 std::ostream &RobotCommand::write(std::ostream &os) const {
     return os << "{"
               << "id: " << formatString("%2i", this->id) << ", "
-              << "velocity: " << this->velocity << ", "
+              << ", position: " << position
+            //  << "velocity: " << this->velocity << ", "
               << "yaw: " << this->yaw << ", "
-              << "targetAngularVel: " << formatString("%5f", this->targetAngularVelocity) << ", "
-              << "useAngularVel: " << (this->useAngularVelocity ? " true" : "false") << ", "
+            //  << "targetAngularVel: " << formatString("%5f", this->targetAngularVelocity) << ", "
+            //  << "useAngularVel: " << (this->useAngularVelocity ? " true" : "false") << ", "
               << "cameraYaw: " << this->cameraYawOfRobot << ", "
               << "cameraYawIsSet: " << (this->cameraYawOfRobotIsSet ? " true" : "false") << ", "
               << "kickSpeed: " << formatString("%5f", this->kickSpeed) << ", "