From 24ac3e09570c8e3ccdb81b357f96b3e365f6b67d Mon Sep 17 00:00:00 2001 From: Ali Salehi Date: Wed, 1 May 2024 21:20:45 +0200 Subject: [PATCH 1/3] Fix grsim forwarder. --- source/cli/app.cpp | 11 ++++- source/sender/CMakeLists.txt | 1 + source/sender/command.h | 18 ++++++++ source/sender/grsim.cpp | 66 +++++++++++------------------ source/sender/grsim.h | 7 ++- source/soccer/internal_finalize.cpp | 2 +- source/soccer/robot/motion_plan.cpp | 2 +- source/soccer/robot/robot.cpp | 19 +++++---- source/soccer/robot/robot.h | 10 +++-- 9 files changed, 75 insertions(+), 61 deletions(-) create mode 100644 source/sender/command.h diff --git a/source/cli/app.cpp b/source/cli/app.cpp index c34e5e99..9bfa3942 100644 --- 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 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 index ef284063..24514581 100644 --- 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 new file mode 100644 index 00000000..39c03450 --- /dev/null +++ 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 \ No newline at end of file diff --git a/source/sender/grsim.cpp b/source/sender/grsim.cpp index 520eab63..87498460 100644 --- 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(); } -#if GRSIM_FIXED -void Grsim::SendData(const Robot *const robots, const int robot_count, bool color) +void Grsim::sendData(const std::vector &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 \ No newline at end of file diff --git a/source/sender/grsim.h b/source/sender/grsim.h index 83822645..6467b907 100644 --- 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& commands); }; } // namespace Tyr::Sender \ No newline at end of file diff --git a/source/soccer/internal_finalize.cpp b/source/soccer/internal_finalize.cpp index 91b70a7f..66767ddf 100644 --- 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/robot/motion_plan.cpp b/source/soccer/robot/motion_plan.cpp index ebd815cc..017364ef 100644 --- 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 ); diff --git a/source/soccer/robot/robot.cpp b/source/soccer/robot/robot.cpp index 3ebb613f..ac48b1ba 100644 --- a/source/soccer/robot/robot.cpp +++ b/source/soccer/robot/robot.cpp @@ -58,7 +58,7 @@ Robot::Robot() CMDindex = 0; for (int i = 0; i < 10; i++) - lastCMDs[i] = Common::Vec3(); + lastCMDs[i] = Sender::Command(); State.velocity.x = 0.0f; State.velocity.y = 0.0f; @@ -148,7 +148,7 @@ Common::Vec3 Robot::ComputeMotionCommand(bool accurate, float speed, VelocityPro if (speed > 100) speed = 100; - Common::Vec3 motion = MotionPlan(State, target, speed, accurate, lastCMDs, velocityProfile); + Common::Vec3 motion = MotionPlan(State, target, speed, accurate, velocityProfile); target.velocity.x = 0; target.velocity.y = 0; @@ -161,9 +161,13 @@ void Robot::MoveByMotion(Common::Vec3 motion) motion.x = std::min(100.0f, std::max(-100.0f, motion.x)); motion.y = std::min(100.0f, std::max(-100.0f, motion.y)); // motion.x=0; - lastCMDs[CMDindex] = motion; - lastCMDs[10].x = CMDindex; - lastCMDs[10].y = PREDICT_CMDS; + + Sender::Command &command = lastCMDs[CMDindex]; + command.motion = motion; + command.current_angle = State.angle; + command.target_angle = target.angle; + + last_cmd_idx = CMDindex; CMDindex++; if (CMDindex > PREDICT_CMDS - 1) @@ -288,9 +292,8 @@ void Robot::makeSendingDataReady() new_comm_ready = true; } -Common::Vec3 Robot::GetCurrentMotionCommand() const +Sender::Command Robot::GetCurrentCommand() const { - const int motion_idx = static_cast(lastCMDs[10].x); - return lastCMDs[motion_idx]; + return lastCMDs[last_cmd_idx]; } } // namespace Tyr::Soccer diff --git a/source/soccer/robot/robot.h b/source/soccer/robot/robot.h index 68289702..21fcd090 100644 --- a/source/soccer/robot/robot.h +++ b/source/soccer/robot/robot.h @@ -29,8 +29,10 @@ class Robot bool halted; bool new_comm_ready; - Common::Vec3 lastCMDs[11]; - int CMDindex; + Sender::Command lastCMDs[11]; + int last_cmd_idx = 0; + + int CMDindex; int remainingPIDParams; float p, i, iMax, torque; @@ -56,7 +58,7 @@ class Robot void face(Common::Vec2 _target); Common::Vec3 MotionPlan(Common::RobotState state, Common::RobotState target, float speed, bool accurate, - Common::Vec3 *cmd, VelocityProfile *velocityProfile); + VelocityProfile *velocityProfile); void Move(bool accurate, float speed, VelocityProfile *velocityProfile); @@ -64,7 +66,7 @@ class Robot Common::Vec3 ComputeMotionCommand(bool accurate, float speed, VelocityProfile *velocityProfile); - Common::Vec3 GetCurrentMotionCommand() const; + Sender::Command GetCurrentCommand() const; void makeSendingDataReady(); }; From 1b813d14747009de8835c2d41b3a2f2147ccf208 Mon Sep 17 00:00:00 2001 From: Ali Salehi Date: Wed, 1 May 2024 23:07:53 +0200 Subject: [PATCH 2/3] Fix asan issue in obstacle map. --- source/soccer/obstacle/obstacle.cpp | 2 +- source/soccer/obstacle/obstacle_map.cpp | 55 +++++++------------------ source/soccer/obstacle/obstacle_new.h | 10 ++--- 3 files changed, 20 insertions(+), 47 deletions(-) diff --git a/source/soccer/obstacle/obstacle.cpp b/source/soccer/obstacle/obstacle.cpp index b12c15ca..6a231693 100644 --- 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 index 7f99618a..eb361bfd 100644 --- 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(_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(_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::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 index 0ab98f42..f4f39968 100644 --- 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> 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); From f3b3d2fc6a2c2e4c183b6508644c3d44189095a8 Mon Sep 17 00:00:00 2001 From: Ali Salehi Date: Wed, 1 May 2024 23:08:27 +0200 Subject: [PATCH 3/3] Change cofigs to grsim by default. --- data/config.toml | 12 ++++++------ source/soccer/robot/robot.cpp | 1 + 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/data/config.toml b/data/config.toml index af3b9dbd..c79abaa6 100644 --- 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/soccer/robot/robot.cpp b/source/soccer/robot/robot.cpp index ac48b1ba..4d14f4c1 100644 --- a/source/soccer/robot/robot.cpp +++ b/source/soccer/robot/robot.cpp @@ -163,6 +163,7 @@ void Robot::MoveByMotion(Common::Vec3 motion) // motion.x=0; Sender::Command &command = lastCMDs[CMDindex]; + command.vision_id = vision_id; command.motion = motion; command.current_angle = State.angle; command.target_angle = target.angle;