Merge pull request #13 from Immortals-Robotics/dev/grsim

Fix grsim support.
Immortals-Robotics · May 1, 2024 · 86a2c8e · 86a2c8e
2 parents b2ddef9 + f3b3d2f
commit 86a2c8e
Show file tree

Hide file tree

Showing 13 changed files with 102 additions and 114 deletions.
diff --git a/data/config.toml b/data/config.toml
@@ -29,8 +29,8 @@ use_kalman_ang = false
 use_camera = [ # TODO #4 Check the cameras to be true
 	true,
 	true,
-	false,
-	false,
+	true,
+	true,
 
 	false,
 	false,
@@ -40,7 +40,7 @@ use_camera = [ # TODO #4 Check the cameras to be true
 
 [soccer]
 # left  = 0, right = 1
-our_side = 0 # TODO #5 Disable the simulation for a higher performance
+our_side = 1 # TODO #5 Disable the simulation for a higher performance
 
 enable_simulation = false #false # TODO #6 Disable the simulation for a higher performance
 command_opp_robots = false # TODO #7 Turn to false, in real games
@@ -169,8 +169,8 @@ init_gk_id  = 3 # TODO #8 Check the robot IDs
 #ip   = "224.5.23.92" # Simulator
 ip   = "224.5.23.2" # Default
 #port = 10999 # Simulator
-#port = 10020 # Default simulator vision
-port = 10006 # Default real vision
+port = 10020 # Default simulator vision
+#port = 10006 # Default real vision
 
 [network.tracker]
 ip   = "224.5.23.2"
@@ -190,7 +190,7 @@ ip   = "127.0.0.1"
 port = 10066
 
 [network.grsim]
-ip   = "192.168.0.112"
+ip   = "127.0.0.1"
 port = 20011
 
 [network.control_simulation]

diff --git a/source/cli/app.cpp b/source/cli/app.cpp
@@ -86,8 +86,15 @@ void Application::aiThreadEentry()
         m_vision->ProcessVision();
         m_ai->Process(m_world_state.get());
 
-        // m_grsim->SendData(m_ai->OwnRobot, Setting::kMaxOnFieldTeamRobots,
-        //                     settings->our_color);
+		std::vector<Sender::Command> commands;
+		commands.reserve(Common::Setting::kMaxOnFieldTeamRobots);
+
+		for (int robot_idx = 0; robot_idx <  Common::Setting::kMaxOnFieldTeamRobots; ++robot_idx)
+		{
+			commands.emplace_back(m_ai->OwnRobot[robot_idx].GetCurrentCommand());
+		}
+
+        m_grsim->sendData(commands);
 
         m_sender->sendAll();
 

diff --git a/source/sender/CMakeLists.txt b/source/sender/CMakeLists.txt
@@ -2,6 +2,7 @@ project(${CMAKE_PROJECT_NAME}-sender)
 
 set(HEADER_FILES
 	pch.h
+	command.h
 	grsim.h
 	sender.h
 	protocol/data_lite.h

diff --git a/source/sender/command.h b/source/sender/command.h
@@ -0,0 +1,18 @@
+#pragma once
+
+namespace Tyr::Sender
+{
+struct Command
+{
+    int vision_id = -1;
+
+    Common::Vec3 motion;
+
+    Common::Angle current_angle;
+    Common::Angle target_angle;
+
+    int shoot = 0;
+    int chip = 0;
+    int dribbler = 0;
+};
+} // namespace Tyr::Sender
diff --git a/source/sender/grsim.cpp b/source/sender/grsim.cpp
@@ -7,70 +7,54 @@ Grsim::Grsim(Common::NetworkAddress address) : address(address)
     socket = std::make_unique<Common::UdpServer>();
 }
 
-#if GRSIM_FIXED
-void Grsim::SendData(const Robot *const robots, const int robot_count, bool color)
+void Grsim::sendData(const std::vector<Command> &commands)
 {
     Protos::grSim_Packet packet;
-    packet.mutable_commands()->set_isteamyellow(color);
+    packet.mutable_commands()->set_isteamyellow(Common::setting().our_color == Common::TeamColor::Yellow);
     packet.mutable_commands()->set_timestamp(0.0);
 
-    for (int robot_idx = 0; robot_idx < robot_count; ++robot_idx)
+    for (const Command &command : commands)
     {
-        const Robot *const robot = robots + robot_idx;
+        Protos::grSim_Robot_Command *proto_command = packet.mutable_commands()->add_robot_commands();
+        proto_command->set_id(command.vision_id);
 
-        Protos::grSim_Robot_Command *command = packet.mutable_commands()->add_robot_commands();
-        command->set_id(robot->vision_id);
+        proto_command->set_wheelsspeed(false);
 
-        command->set_wheelsspeed(false);
-        /*command->set_wheel1(edtV1->text().toDouble());
-        command->set_wheel2(edtV2->text().toDouble());
-        command->set_wheel3(edtV3->text().toDouble());
-        command->set_wheel4(edtV4->text().toDouble());*/
+        const Common::Vec3 motion = command.motion;
 
-        const int          cmd_idx   = robots[robot_idx].lastCMDs[10].x;
-        const Common::Vec3 motion    = robots[robot_idx].lastCMDs[cmd_idx];
-        float              robot_ang = (90 - robot->State.Angle) * 3.1415 / 180.0;
-        float              new_VelX  = motion.x * cos(robot_ang) - motion.y * sin(robot_ang);
-        float              new_VelY  = motion.x * sin(robot_ang) + motion.y * cos(robot_ang);
+        float robot_ang = (Common::Angle::fromDeg(90.0f) - command.current_angle).rad();
 
-        command->set_veltangent(new_VelY / 20.0);
-        command->set_velnormal(-new_VelX / 20.0);
+        float new_VelX = motion.x * cos(robot_ang) - motion.y * sin(robot_ang);
+        float new_VelY = motion.x * sin(robot_ang) + motion.y * cos(robot_ang);
 
-        float w = robot->target.Angle - robot->State.Angle;
-        while (w > 180)
-        {
-            w -= 360;
-        }
-        while (w < -180)
-        {
-            w += 360;
-        }
+        proto_command->set_veltangent(new_VelY / 20.0);
+        proto_command->set_velnormal(-new_VelX / 20.0);
+
+        float w = (command.target_angle - command.current_angle).deg();
         w /= 10.0f;
 
-        command->set_velangular(w);
-        // command->set_velangular(0);
+        proto_command->set_velangular(w);
 
-        if (robot->shoot > 0)
+        if (command.shoot > 0)
         {
-            command->set_kickspeedx(robot->shoot / 10.f);
-            command->set_kickspeedz(0);
+            proto_command->set_kickspeedx(command.shoot / 10.f);
+            proto_command->set_kickspeedz(0);
         }
-        else if (robot->chip > 0)
+        else if (command.chip > 0)
         {
-            float chip = 0.f; // robot->chip / 25.0f;
-            command->set_kickspeedx(chip * 0.707f);
-            command->set_kickspeedz(chip / 0.707f);
+            float chip = command.chip / 25.0f;
+            proto_command->set_kickspeedx(chip * 0.707f);
+            proto_command->set_kickspeedz(chip / 0.707f);
         }
         else
         {
-            command->set_kickspeedx(.0f);
-            command->set_kickspeedz(.0f);
+            proto_command->set_kickspeedx(.0f);
+            proto_command->set_kickspeedz(.0f);
         }
 
-        command->set_spinner(robot->dribbler);
+        proto_command->set_spinner(command.dribbler);
 
         socket->send(packet, address);
     }
 }
-#endif
 } // namespace Tyr::Sender
diff --git a/source/sender/grsim.h b/source/sender/grsim.h
@@ -1,5 +1,7 @@
 #pragma once
 
+#include "command.h"
+
 namespace Tyr::Sender
 {
 class Grsim
@@ -12,9 +14,6 @@ class Grsim
     Grsim(Common::NetworkAddress address);
     ~Grsim() = default;
 
-// TODO: fix and remove
-#if GRSIM_FIXED
-    void sendData(const Robot *const robots, const int robot_count, bool color);
-#endif
+    void sendData(const std::vector<Command>& commands);
 };
 } // namespace Tyr::Sender
diff --git a/source/soccer/internal_finalize.cpp b/source/soccer/internal_finalize.cpp
@@ -30,7 +30,7 @@ void Ai::internalFinalize(Common::WorldState *worldState)
             continue;
         for (int j = 0; j < 11; j++) // kheyli tof malie...
         {
-            worldState->lastCMDS[OwnRobot[i].State.vision_id][j] = OwnRobot[i].lastCMDs[j];
+            worldState->lastCMDS[OwnRobot[i].State.vision_id][j] = OwnRobot[i].lastCMDs[j].motion;
         }
     }
 }

diff --git a/source/soccer/obstacle/obstacle.cpp b/source/soccer/obstacle/obstacle.cpp
@@ -64,7 +64,7 @@ void AddRectangle ( int x , int y , int w , int h )
         }
 }*/
 
-ObsMap map(100);
+ObsMap map{};
 
 void clear_map()
 {

diff --git a/source/soccer/obstacle/obstacle_map.cpp b/source/soccer/obstacle/obstacle_map.cpp
@@ -2,45 +2,31 @@
 
 namespace Tyr::Soccer
 {
-ObsMap::ObsMap(unsigned int _maxObs)
-{
-    obstacle = new BaseObstacle *[_maxObs];
-
-    obsNum = 0;
-    maxObs = _maxObs;
-}
+ObsMap::ObsMap()
+{}
 
 void ObsMap::AddCircle(float _x, float _y, float _r)
 {
-    if ((_r > 0) && (obsNum < maxObs))
+    // TODO: verify if this check is needed
+    if (_r > 0)
     {
-        obstacle[obsNum++] = new CircleObstacle(_x, _y, _r);
+        m_obstacles.emplace_back(std::make_unique<CircleObstacle>(_x, _y, _r));
     }
 }
 void ObsMap::AddRectangle(float _x, float _y, float _w, float _h)
 {
-    if (obsNum < maxObs)
-    {
-        obstacle[obsNum++] = new RectangleObstacle(_x, _y, _w, _h);
-    }
-}
-
-void ObsMap::AddObstacle(BaseObstacle *obs)
-{
-    if ((obs) && (obsNum < maxObs))
-    {
-        obstacle[obsNum++] = obs;
-    }
+    m_obstacles.emplace_back(std::make_unique<RectangleObstacle>(_x, _y, _w, _h));
 }
 
 bool ObsMap::IsInObstacle(float _x, float _y)
 {
-    for (int i = 0; i < obsNum; i++)
+    for (int i = 0; i < getObsNum(); i++)
     {
-        if (obstacle[i]->IsInObstacle(_x, _y))
+        if (m_obstacles[i]->IsInObstacle(_x, _y))
             return true;
     }
-    if (std::fabs(_x) > 10000 || std::fabs(_y) > 10000) // If the  values where getting to much (this fixes the errors for now...
+    if (std::fabs(_x) > 10000 ||
+        std::fabs(_y) > 10000) // If the  values where getting to much (this fixes the errors for now...
     {
         return true;
     }
@@ -49,13 +35,11 @@ bool ObsMap::IsInObstacle(float _x, float _y)
 
 float ObsMap::NearestDistance(float _x, float _y)
 {
-    if (obsNum == 0)
-        return INT_MAX;
-    float dis = obstacle[0]->NearestDistance(_x, _y);
+    float dis = std::numeric_limits<float>::max();
     float tmp_dis;
-    for (int i = 0; i < obsNum; i++)
+    for (int i = 0; i < getObsNum(); i++)
     {
-        tmp_dis = obstacle[i]->NearestDistance(_x, _y);
+        tmp_dis = m_obstacles[i]->NearestDistance(_x, _y);
         if (tmp_dis < dis)
             dis = tmp_dis;
         if (dis <= 0)
@@ -67,20 +51,11 @@ float ObsMap::NearestDistance(float _x, float _y)
 
 void ObsMap::resetMap()
 {
-    for (int i = 0; i < obsNum; i++)
-    {
-        delete obstacle[i];
-    }
-
-    free(obstacle);
-
-    obstacle = new BaseObstacle *[maxObs];
-
-    obsNum = 0;
+    m_obstacles.clear();
 }
 
 int ObsMap::getObsNum()
 {
-    return obsNum;
+    return m_obstacles.size();
 }
 } // namespace Tyr::Soccer
diff --git a/source/soccer/obstacle/obstacle_new.h b/source/soccer/obstacle/obstacle_new.h
@@ -5,6 +5,8 @@ namespace Tyr::Soccer
 class BaseObstacle
 {
 public:
+    virtual ~BaseObstacle() = default;
+
     virtual bool  IsInObstacle(float _x, float _y)    = 0;
     virtual float NearestDistance(float _x, float _y) = 0;
 };
@@ -47,17 +49,13 @@ class HalfPlaneObstacle : public BaseObstacle
 class ObsMap
 {
 private:
-    unsigned int obsNum, maxObs;
-
-    BaseObstacle **obstacle;
+    std::vector<std::unique_ptr<BaseObstacle>> m_obstacles;
 
 public:
-    ObsMap(unsigned int _maxObs);
+    ObsMap();
 
     void AddCircle(float _x, float _y, float _r);
     void AddRectangle(float _x, float _y, float _w, float _h);
-    // void AddHalfPlane ( float _x , float _y , float _w , float _h );
-    void AddObstacle(BaseObstacle *obs);
 
     bool  IsInObstacle(float _x, float _y);
     float NearestDistance(float _x, float _y);

diff --git a/source/soccer/robot/motion_plan.cpp b/source/soccer/robot/motion_plan.cpp
@@ -5,7 +5,7 @@ namespace Tyr::Soccer
 Common::Vec2 vdes_ann;
 
 Common::Vec3 Robot::MotionPlan(Common::RobotState state, Common::RobotState target, float speed, bool accurate,
-                               Common::Vec3 *cmd, VelocityProfile *velocityProfile)
+                               VelocityProfile *velocityProfile)
 {
     /*Common::Vec2 max_spd = Common::Vec2 ( 100.0f );
     Common::Vec2 max_dec = Common::Vec2 ( 2.3f );