From 8dab773e8da2f5064e4e6aed3da70c2107fb149f Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 18:56:56 +0100 Subject: [PATCH 01/81] servo value --- src/modelec_strat/src/action/down_action.cpp | 10 +++++----- src/modelec_strat/src/action/up_action.cpp | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 18602b9..fe23e92 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -34,7 +34,7 @@ void Modelec::DownAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 1.95; + msg.items[0].start_angle = 1.75; msg.items[0].end_angle = 3; msg.items[0].duration_s = 1; @@ -44,14 +44,14 @@ void Modelec::DownAction::Next() msg.items[1].duration_s = 1; msg.items[2].id = 2; - msg.items[2].start_angle = 0.3; + msg.items[2].start_angle = 0.2; msg.items[2].end_angle = 0; - msg.items[2].duration_s = 0.5; + msg.items[2].duration_s = 0.2; msg.items[3].id = 3; - msg.items[3].start_angle = 2.7; + msg.items[3].start_angle = 2.8; msg.items[3].end_angle = 2.9; - msg.items[3].duration_s = 0.5; + msg.items[3].duration_s = 0.2; } if (front_ == BACK || front_ == BOTH) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 0907bba..ded689e 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -35,7 +35,7 @@ void Modelec::UPAction::Next() { msg.items[0].id = 0; msg.items[0].start_angle = 3; - msg.items[0].end_angle = 1.95; + msg.items[0].end_angle = 1.75; msg.items[0].duration_s = 1; msg.items[1].id = 1; @@ -44,13 +44,13 @@ void Modelec::UPAction::Next() msg.items[1].duration_s = 1; msg.items[2].id = 2; - msg.items[2].start_angle = 0; - msg.items[2].end_angle = 0.3; + msg.items[2].start_angle = 0.1; + msg.items[2].end_angle = 0.2; msg.items[2].duration_s = 1; msg.items[3].id = 3; msg.items[3].start_angle = 2.9; - msg.items[3].end_angle = 2.7; + msg.items[3].end_angle = 2.8; msg.items[3].duration_s = 1; } From 4769c012c271e5d95c7629c9e8b1501807712a3a Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 19:22:27 +0100 Subject: [PATCH 02/81] debug lidar --- src/modelec_core/launch/modelec.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 62a073d..2b5dcb9 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -39,7 +39,8 @@ def create_lidar_with_restart(): 'angle_compensate': angle_compensate, 'scan_mode': scan_mode, }], - output='screen' + output='screen', + prefix = ['xterm -e gdb -ex run --args'] ) # Instead of recursion at definition time, we delay the re-creation using a lambda @@ -49,7 +50,7 @@ def create_lidar_with_restart(): on_exit=[ LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 2s...'), TimerAction( - period=0.3, + period=10, actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())] ) ] From 011bba3dbbc19c1a715513fc5f9d63605e40260e Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 19:25:16 +0100 Subject: [PATCH 03/81] debug lidar --- src/modelec_core/launch/modelec.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 2b5dcb9..ee4aa03 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -50,7 +50,7 @@ def create_lidar_with_restart(): on_exit=[ LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 2s...'), TimerAction( - period=10, + period=10.0, actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())] ) ] From f3640ed0f9025b8d0b080ea4a8be4a60b2fc5f90 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 19:26:12 +0100 Subject: [PATCH 04/81] debug lidar --- src/modelec_core/launch/modelec.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index ee4aa03..955a8b0 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -100,7 +100,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/dev/USB_ODO", + 'serial_port': "/tmp/USB_ODO", 'baudrate': 115200, 'name': "pcb_odo", }] @@ -110,7 +110,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/dev/USB_ACTION", + 'serial_port': "/tmp/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] From b232becfe78295cfb009ef1c4811b2ba947e7e2b Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 19:28:13 +0100 Subject: [PATCH 05/81] debug lidar --- src/modelec_core/launch/modelec.launch.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 955a8b0..764bbf4 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -39,8 +39,7 @@ def create_lidar_with_restart(): 'angle_compensate': angle_compensate, 'scan_mode': scan_mode, }], - output='screen', - prefix = ['xterm -e gdb -ex run --args'] + output='screen' ) # Instead of recursion at definition time, we delay the re-creation using a lambda @@ -50,7 +49,7 @@ def create_lidar_with_restart(): on_exit=[ LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 2s...'), TimerAction( - period=10.0, + period=2.0, actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())] ) ] From cd36e1d392ccf5b9d1fdc8114c6d469ffbeb2f4f Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 20:02:47 +0100 Subject: [PATCH 06/81] debug lidar --- src/modelec_core/launch/modelec.launch.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 764bbf4..7269c7a 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -19,7 +19,7 @@ def generate_launch_description(): frame_id = LaunchConfiguration('frame_id', default='laser') inverted = LaunchConfiguration('inverted', default='false') angle_compensate = LaunchConfiguration('angle_compensate', default='true') - scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') + scan_mode = LaunchConfiguration('scan_mode', default='Standard') # ---------------------------- # Infinite LIDAR restart logic @@ -99,7 +99,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/tmp/USB_ODO", + 'serial_port': "/dev/USB_ODO", 'baudrate': 115200, 'name': "pcb_odo", }] @@ -109,7 +109,7 @@ def launch_com(context, *args, **kwargs): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/tmp/USB_ACTION", + 'serial_port': "/dev/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] From 1707b8c7209edb9b58fb99d5f2cbc7d899f22864 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 21:14:55 +0100 Subject: [PATCH 07/81] new game controller sys --- .../msg/Action/ActionExec.msg | 4 + src/modelec_strat/CMakeLists.txt | 1 + .../action/toggle_servo_action.hpp | 40 ++++++ .../include/modelec_strat/action_executor.hpp | 16 +++ src/modelec_strat/src/action/free_action.cpp | 4 +- src/modelec_strat/src/action/take_action.cpp | 4 +- .../src/action/toggle_servo_action.cpp | 92 +++++++++++++ src/modelec_strat/src/action_executor.cpp | 125 +++++++++++++++++- 8 files changed, 275 insertions(+), 11 deletions(-) create mode 100644 src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp create mode 100644 src/modelec_strat/src/action/toggle_servo_action.cpp diff --git a/src/modelec_interfaces/msg/Action/ActionExec.msg b/src/modelec_interfaces/msg/Action/ActionExec.msg index 24367e6..bd8d70f 100644 --- a/src/modelec_interfaces/msg/Action/ActionExec.msg +++ b/src/modelec_interfaces/msg/Action/ActionExec.msg @@ -3,8 +3,10 @@ string DELIMITER = ";" # Action string UP = "UP" string DOWN = "DOWN" +string TOGGLE_ARM = "TOGGLE_ARM" string TAKE = "TAKE" string FREE = "FREE" +string TOGGLE_SERVO = "TOGGLE_SERVO" string THERMO_DEPLOY = "THERMO_DEPLOY" string THERMO_UNDEPLOY = "THERMO_UNDEPLOY" string MAX_DEPLOY = "MAX_DEPLOY" @@ -17,6 +19,8 @@ int32 TAKE_STEP = 3 int32 FREE_STEP = 4 int32 THERMO_DEPLOY_STEP = 5 int32 THERMO_UNDEPLOY_STEP = 6 +int32 TOGGLE_SERVO_STEP = 7 +int32 TOGGLE_ARM_STEP = 8 int32 MAX_DEPLOY_STEP = 99 int32[] steps diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index b51d79a..bd2cdfb 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -25,6 +25,7 @@ set(strat_fsm_sources src/action/down_action.cpp src/action/free_action.cpp src/action/take_action.cpp + src/action/toggle_servo_action.cpp src/missions/go_home_mission.cpp src/missions/take_mission.cpp diff --git a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp new file mode 100644 index 0000000..d27c438 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + class ToggleServoAction : public BaseAction + { + public: + ToggleServoAction(const std::shared_ptr& action_executor); + ToggleServoAction(const std::shared_ptr& action_executor, Front front, int n); + ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo); + ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos); + + void Next() override; + void Init(const std::vector& params) override; + void AddServo(int id, Front front); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); + + inline static const std::string Name = ActionExec::TOGGLE_SERVO; + + private: + std::vector> servos_; + + std::queue steps_; + + inline static const bool registered_ = + []() + { + BaseAction::Registry()[Name] = + [](const std::shared_ptr& exec) + { + return std::make_shared(exec); + }; + return true; + }(); + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 6186b01..d58b7ac 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -35,16 +35,29 @@ namespace Modelec void Up(BaseAction::Front front, bool force = false); + void ToggleArm(BaseAction::Front front, bool force = false); + void Take(const std::vector>& servos, bool force = false); void Free(const std::vector>& servos, bool force = false); + void ToggleServo(const std::vector>& servos, bool force = false); + void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); std::array, 2> box_obstacles_; + std::array servo_pos_; + + struct ArmState + { + bool down; + bool rotated; + }; + std::array arm_pos_; + bool IsEmpty() const; bool IsFull() const; @@ -84,6 +97,9 @@ namespace Modelec int current_step_; + float last_left_trig = 1.0f; + float last_right_trig = 1.0f; + private: rclcpp::Node::SharedPtr node_; }; diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 52fe403..8bfdce9 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -45,8 +45,8 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = servos_[i].second ? 3 : 0; - msg.items[i].end_angle = servos_[i].second ? 1 : 0; + msg.items[i].start_angle = 3; + msg.items[i].end_angle = 1; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 25849a6..9c66ac1 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -45,8 +45,8 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = servos_[i].second ? 1 : 0; - msg.items[i].end_angle = servos_[i].second ? 3 : 0; + msg.items[i].start_angle = 1; + msg.items[i].end_angle = 3; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/toggle_servo_action.cpp b/src/modelec_strat/src/action/toggle_servo_action.cpp new file mode 100644 index 0000000..5ed9f8a --- /dev/null +++ b/src/modelec_strat/src/action/toggle_servo_action.cpp @@ -0,0 +1,92 @@ +#include + +#include "modelec_strat/action_executor.hpp" + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +{ + steps_.push(ActionExec::TOGGLE_SERVO_STEP); + steps_.push(ActionExec::DONE_STEP); +} + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, Front front, int n) : ToggleServoAction(action_executor) +{ + AddServo(n, front); +} + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo) : ToggleServoAction(action_executor) +{ + AddServo(servo.first, servo.second); +} + +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos) : ToggleServoAction(action_executor) +{ + AddServos(servos); +} + +void Modelec::ToggleServoAction::Next() +{ + if (steps_.empty()) + { + done_ = true; + return; + } + + auto step = steps_.front(); + steps_.pop(); + + switch (step) + { + case ActionExec::TOGGLE_SERVO_STEP: + { + modelec_interfaces::msg::ActionServoTimedArray msg; + + msg.items.resize(servos_.size()); + + for (size_t i = 0; i < servos_.size(); i++) + { + msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); + msg.items[i].start_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 1 : 3; + msg.items[i].end_angle = action_executor_->servo_pos_[servos_[i].first + (servos_[i].second ? 0 : 4)] ? 3 : 1; + msg.items[i].duration_s = 0.5; + } + + action_executor_->MoveServoTimed(msg); + } + break; + case ActionExec::DONE_STEP: + { + done_ = true; + } + break; + default: + break; + } +} + +void Modelec::ToggleServoAction::Init(const std::vector& params) +{ + if (params.size() >= 2) + { + for (size_t i = 1; i < params.size(); i += 2) + { + int id = std::stoi(params[i]); + bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; + AddServo(id, front ? FRONT : BACK); + } + } +} + +void Modelec::ToggleServoAction::AddServo(int id, Front front) +{ + servos_.emplace_back(id, front); +} + +void Modelec::ToggleServoAction::AddServo(std::pair servo) +{ + servos_.emplace_back(servo); +} + +void Modelec::ToggleServoAction::AddServos(const std::vector>& servos) +{ + servos_.insert(servos_.end(), servos.begin(), servos.end()); +} diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index d41a7cc..28dc670 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -4,6 +4,7 @@ #include "modelec_strat/action/up_action.hpp" #include "modelec_strat/action/free_action.hpp" #include "modelec_strat/action/take_action.hpp" +#include "modelec_strat/action/toggle_servo_action.hpp" #include "modelec_utils/utils.hpp" namespace Modelec @@ -84,23 +85,83 @@ namespace Modelec "/joy", 10, [this](const sensor_msgs::msg::Joy::SharedPtr msg) { // use game controller to manually control all the action. make it carefully - if (msg->buttons.size() >= 5) + if (msg->buttons.size() >= 15) { if (msg->buttons[0] == 1) // A button { - Down(BaseAction::BOTH); + ToggleServo({{0, BaseAction::FRONT}}); } else if (msg->buttons[1] == 1) // B button { - Up(BaseAction::BOTH); + ToggleServo({{1, BaseAction::FRONT}}); } else if (msg->buttons[3] == 1) // X button { - Take({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); + ToggleServo({{2, BaseAction::FRONT}}); } else if (msg->buttons[4] == 1) // Y button { - Free({{0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}}); + ToggleServo({{3, BaseAction::FRONT}}); + } + else if (msg->buttons[6] == 1) // L1 button + { + // ROTATE BACK + } + else if (msg->buttons[7] == 1) // R1 button + { + // ROTATE FRONT + } + else if (msg->buttons[14] == 1) // LT button + { + // IDK + } + else if (msg->buttons[15] == 1) // LT button + { + // IDK + } + } + if (msg->axes.size() == 8) + { + auto left_trig = msg->axes[4]; + auto right_trig = msg->axes[5]; + + if (left_trig == 1 && last_left_trig == -1) // left trigger pressed + { + ToggleArm(BaseAction::BACK); + last_left_trig = left_trig; + } else if (left_trig == -1 && last_left_trig == 1) // left trigger released + { + last_left_trig = left_trig; + } + + if (right_trig == 1 && last_left_trig == -1) // right trigger pressed + { + ToggleArm(BaseAction::FRONT); + last_right_trig = right_trig; + } else if (right_trig == -1 && last_right_trig == 1) // right trigger released + { + last_right_trig = right_trig; + } + + auto btn_horizontal = msg->axes[6]; + auto btn_vertical = msg->axes[7]; + + if (btn_horizontal == 1.0) // left + { + ToggleServo({{3, BaseAction::BACK}}); + } + else if (btn_horizontal == -1.0) // right + { + ToggleServo({{1, BaseAction::BACK}}); + } + + if (btn_vertical == 1.0) // up + { + ToggleServo({{0, BaseAction::BACK}}); + } + else if (btn_vertical == -1.0) // down + { + ToggleServo({{2, BaseAction::BACK}}); } } }); @@ -150,7 +211,7 @@ namespace Modelec } void ActionExecutor::Down(BaseAction::Front front, bool force) { - if (action_done_ || force) + if ((action_done_ && !arm_pos_[front].down) || force) { action_ = std::make_shared(shared_from_this(), front); if (action_done_) @@ -159,12 +220,14 @@ namespace Modelec } action_done_ = false; + arm_pos_[front].down = true; + Update(); } } void ActionExecutor::Up(BaseAction::Front front, bool force) { - if (action_done_ || force) + if ((action_done_ && arm_pos_[front].down) || force) { action_ = std::make_shared(shared_from_this(), front); if (action_done_) @@ -173,10 +236,27 @@ namespace Modelec } action_done_ = false; + arm_pos_[front].down = false; + Update(); } } + void ActionExecutor::ToggleArm(BaseAction::Front front, bool force) + { + if (action_done_ || force) + { + if (arm_pos_[front].down) + { + Up(front, force); + } + else + { + Down(front, force); + } + } + } + void ActionExecutor::Take(const std::vector>& servos, bool force) { if (action_done_ || force) { @@ -187,6 +267,12 @@ namespace Modelec } action_done_ = false; + + for (auto servo : servos) + { + servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = true; + } + Update(); } } @@ -201,6 +287,31 @@ namespace Modelec } action_done_ = false; + for (auto servo : servos) + { + servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = false; + } + + Update(); + } + } + + void ActionExecutor::ToggleServo(const std::vector>& servos, bool force) + { + if (action_done_ || force) + { + action_ = std::make_shared(shared_from_this(), servos); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + for (auto servo : servos) + { + servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = !servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)]; + } + Update(); } } From c515b849f12effc2613d3caee3b2a28e3d7bfab7 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 21:27:30 +0100 Subject: [PATCH 08/81] new game controller sys --- src/modelec_strat/src/action_executor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 28dc670..f74e4e4 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -97,11 +97,11 @@ namespace Modelec } else if (msg->buttons[3] == 1) // X button { - ToggleServo({{2, BaseAction::FRONT}}); + ToggleServo({{3, BaseAction::FRONT}}); } else if (msg->buttons[4] == 1) // Y button { - ToggleServo({{3, BaseAction::FRONT}}); + ToggleServo({{2, BaseAction::FRONT}}); } else if (msg->buttons[6] == 1) // L1 button { From 40f7ce5810267f33170451a0ad2b0ae4748490e0 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 22:50:39 +0100 Subject: [PATCH 09/81] new game controller sys --- .../msg/Action/ActionExec.msg | 2 + src/modelec_strat/CMakeLists.txt | 1 + .../action/rotate_arm_action.hpp | 39 ++++++++ .../include/modelec_strat/action_executor.hpp | 2 + .../src/action/rotate_arm_action.cpp | 92 +++++++++++++++++++ src/modelec_strat/src/action_executor.cpp | 24 ++++- 6 files changed, 157 insertions(+), 3 deletions(-) create mode 100644 src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp create mode 100644 src/modelec_strat/src/action/rotate_arm_action.cpp diff --git a/src/modelec_interfaces/msg/Action/ActionExec.msg b/src/modelec_interfaces/msg/Action/ActionExec.msg index bd8d70f..a7841fd 100644 --- a/src/modelec_interfaces/msg/Action/ActionExec.msg +++ b/src/modelec_interfaces/msg/Action/ActionExec.msg @@ -4,6 +4,7 @@ string DELIMITER = ";" string UP = "UP" string DOWN = "DOWN" string TOGGLE_ARM = "TOGGLE_ARM" +string ROTATE_ARM = "ROTATE_ARM" string TAKE = "TAKE" string FREE = "FREE" string TOGGLE_SERVO = "TOGGLE_SERVO" @@ -21,6 +22,7 @@ int32 THERMO_DEPLOY_STEP = 5 int32 THERMO_UNDEPLOY_STEP = 6 int32 TOGGLE_SERVO_STEP = 7 int32 TOGGLE_ARM_STEP = 8 +int32 ROTATE_ARM_STEP = 9 int32 MAX_DEPLOY_STEP = 99 int32[] steps diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index bd2cdfb..3d67b68 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -26,6 +26,7 @@ set(strat_fsm_sources src/action/free_action.cpp src/action/take_action.cpp src/action/toggle_servo_action.cpp + src/action/rotate_arm_action.cpp src/missions/go_home_mission.cpp src/missions/take_mission.cpp diff --git a/src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp b/src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp new file mode 100644 index 0000000..26b6f47 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + class RotateArmAction : public BaseAction + { + public: + RotateArmAction(const std::shared_ptr& action_executor); + RotateArmAction(const std::shared_ptr& action_executor, Front front, bool rotated = false); + + void Next() override; + void Init(const std::vector& params) override; + void SetFront(Front front); + void SetRotated(bool rotated); + + inline static const std::string Name = ActionExec::TOGGLE_ARM; + + private: + Front front_; + + bool rotated_ = false; + + std::queue steps_; + + inline static const bool registered_ = + []() + { + BaseAction::Registry()[Name] = + [](const std::shared_ptr& exec) + { + return std::make_shared(exec); + }; + return true; + }(); + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index d58b7ac..ace20f7 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -37,6 +37,8 @@ namespace Modelec void ToggleArm(BaseAction::Front front, bool force = false); + void RotateArm(BaseAction::Front front, bool force = false, bool rotated = false); + void Take(const std::vector>& servos, bool force = false); void Free(const std::vector>& servos, bool force = false); diff --git a/src/modelec_strat/src/action/rotate_arm_action.cpp b/src/modelec_strat/src/action/rotate_arm_action.cpp new file mode 100644 index 0000000..e261941 --- /dev/null +++ b/src/modelec_strat/src/action/rotate_arm_action.cpp @@ -0,0 +1,92 @@ +#include + +#include "modelec_strat/action_executor.hpp" + +Modelec::RotateArmAction::RotateArmAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +{ + steps_.push(ActionExec::ROTATE_ARM_STEP); + steps_.push(ActionExec::DONE_STEP); +} + +Modelec::RotateArmAction::RotateArmAction(const std::shared_ptr& action_executor, Front front, bool rotated) + : RotateArmAction(action_executor) +{ + front_ = front; + rotated_ = rotated; +} + +void Modelec::RotateArmAction::Next() +{ + if (steps_.empty()) + { + done_ = true; + return; + } + + auto step = steps_.front(); + steps_.pop(); + + switch (step) + { + case ActionExec::ROTATE_ARM_STEP: + { + ActionServoTimedArray msg; + msg.items.resize(2); + + rotated_ = !rotated_; + + if (rotated_) + { + msg.items[0].id = 2 + front_ == FRONT ? 0 : 8; + msg.items[0].start_angle = front_ == BaseAction::FRONT ? 0.2 : 0; + msg.items[0].end_angle = front_ == BaseAction::FRONT ? 0 : 0; + msg.items[0].duration_s = 1; + + msg.items[1].id = 3 + front_ == FRONT ? 0 : 8; + msg.items[1].start_angle = front_ == BaseAction::FRONT ? 2.8 : 0; + msg.items[1].end_angle = front_ == BaseAction::FRONT ? 0 : 0; + msg.items[1].duration_s = 1; + } else + { + msg.items[0].id = 2 + front_ == FRONT ? 0 : 8; + msg.items[0].start_angle = front_ == BaseAction::FRONT ? 0 : 0; + msg.items[0].end_angle = front_ == BaseAction::FRONT ? 0.2 : 0; + msg.items[0].duration_s = 1; + + msg.items[1].id = 3 + front_ == FRONT ? 0 : 8; + msg.items[1].start_angle = front_ == BaseAction::FRONT ? 0 : 0; + msg.items[1].end_angle = front_ == BaseAction::FRONT ? 2.8 : 0; + msg.items[1].duration_s = 1; + } + + action_executor_->MoveServoTimed(msg); + } + break; + case ActionExec::DONE_STEP: + { + done_ = true; + } + break; + default: + break; + } +} + +void Modelec::RotateArmAction::Init(const std::vector& params) +{ + if (!params.empty()) + { + SetFront(static_cast(std::stoi(params[1]))); + SetRotated(params.size() >= 3 ? (params[2] == "1" || params[2] == "true") : false); + } +} + +void Modelec::RotateArmAction::SetFront(Front front) +{ + front_ = front; +} + +void Modelec::RotateArmAction::SetRotated(bool rotated) +{ + rotated_ = rotated; +} \ No newline at end of file diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index f74e4e4..516a8ad 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -5,6 +5,7 @@ #include "modelec_strat/action/free_action.hpp" #include "modelec_strat/action/take_action.hpp" #include "modelec_strat/action/toggle_servo_action.hpp" +#include "modelec_strat/action/rotate_arm_action.hpp" #include "modelec_utils/utils.hpp" namespace Modelec @@ -105,17 +106,17 @@ namespace Modelec } else if (msg->buttons[6] == 1) // L1 button { - // ROTATE BACK + RotateArm(BaseAction::BACK, false, !arm_pos_[BaseAction::BACK].rotated); } else if (msg->buttons[7] == 1) // R1 button { - // ROTATE FRONT + RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated); } else if (msg->buttons[14] == 1) // LT button { // IDK } - else if (msg->buttons[15] == 1) // LT button + else if (msg->buttons[15] == 1) // LR button { // IDK } @@ -257,6 +258,23 @@ namespace Modelec } } + void ActionExecutor::RotateArm(BaseAction::Front front, bool force, bool rotated) + { + if (action_done_ || force) + { + action_ = std::make_shared(shared_from_this(), front, rotated); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + arm_pos_[front].rotated = rotated; + + Update(); + } + } + void ActionExecutor::Take(const std::vector>& servos, bool force) { if (action_done_ || force) { From f9400e25f3b2f5c5354d3420ee38d3e853414363 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 28 Jan 2026 22:58:53 +0100 Subject: [PATCH 10/81] new game controller sys --- src/modelec_strat/src/action_executor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 516a8ad..0862e7e 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -260,7 +260,7 @@ namespace Modelec void ActionExecutor::RotateArm(BaseAction::Front front, bool force, bool rotated) { - if (action_done_ || force) + if ((action_done_ && arm_pos_[front].rotated != rotated) || force) { action_ = std::make_shared(shared_from_this(), front, rotated); if (action_done_) From f2e45aa65d40d938312836d681404ba243882f5d Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 11:32:14 +0100 Subject: [PATCH 11/81] rewrite action sys hope it do not break anything but can now queue action --- .../include/modelec_strat/action_executor.hpp | 8 +- src/modelec_strat/src/action_executor.cpp | 149 ++++++++++-------- src/modelec_strat/src/strat_fms.cpp | 2 +- 3 files changed, 84 insertions(+), 75 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index ace20f7..07436b4 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -39,11 +39,11 @@ namespace Modelec void RotateArm(BaseAction::Front front, bool force = false, bool rotated = false); - void Take(const std::vector>& servos, bool force = false); + void Take(const std::vector>& servos); - void Free(const std::vector>& servos, bool force = false); + void Free(const std::vector>& servos); - void ToggleServo(const std::vector>& servos, bool force = false); + void ToggleServo(const std::vector>& servos); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); @@ -86,7 +86,7 @@ namespace Modelec rclcpp::Subscription::SharedPtr action_exec_sub_; rclcpp::Subscription::SharedPtr joy_sub_; - std::shared_ptr action_; + std::queue> action_; bool action_done_ = true; int step_running_ = 0; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 0862e7e..0d3922f 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -48,7 +48,7 @@ namespace Modelec { auto token = split(msg->action, ActionExec::DELIMITER[0]); - if (token.size() == 0) + if (token.empty()) { RCLCPP_WARN( node_->get_logger(), @@ -56,10 +56,10 @@ namespace Modelec return; } - action_ = BaseAction::CreateAction( + auto action = BaseAction::CreateAction( token[0], shared_from_this()); - if (action_ == nullptr) + if (action == nullptr) { RCLCPP_WARN( node_->get_logger(), @@ -67,7 +67,8 @@ namespace Modelec msg->action.c_str()); return; } - action_->Init(token); + action->Init(token); + action_.push(action); action_done_ = false; step_running_ = 0; Update(); @@ -180,27 +181,39 @@ namespace Modelec void ActionExecutor::Update() { - if (action_ != nullptr && !action_->IsDone() && step_running_ <= 0) + auto action = action_.front(); + if (action != nullptr && !action->IsDone() && step_running_ <= 0) { RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Update: Executing next step of action, step_running_=%d", step_running_); - action_->Next(); - if (action_->IsDone()) + action->Next(); + if (action->IsDone()) { - action_done_ = true; - action_ = nullptr; + action_.pop(); + if (action_.empty()) + { + action_done_ = true; + } else + { + Update(); + } } } - else if (action_ != nullptr && action_->IsDone()) + else if (action != nullptr && action->IsDone()) { RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Update: Action is done, step_running_=%d", step_running_); - action_done_ = true; - action_ = nullptr; + action_.pop(); + if (action_.empty()) + { + action_done_ = true; + } else { + Update(); + } } } @@ -208,13 +221,16 @@ namespace Modelec { action_done_ = true; step_running_ = 0; - action_ = nullptr; + + std::queue> empty; + std::swap(action_, empty); } void ActionExecutor::Down(BaseAction::Front front, bool force) { - if ((action_done_ && !arm_pos_[front].down) || force) + if ((!arm_pos_[front].down) || force) { - action_ = std::make_shared(shared_from_this(), front); + auto action = std::make_shared(shared_from_this(), front); + action_.push(action); if (action_done_) { step_running_ = 0; @@ -228,9 +244,10 @@ namespace Modelec } void ActionExecutor::Up(BaseAction::Front front, bool force) { - if ((action_done_ && arm_pos_[front].down) || force) + if ((arm_pos_[front].down) || force) { - action_ = std::make_shared(shared_from_this(), front); + auto action = std::make_shared(shared_from_this(), front); + action_.push(action); if (action_done_) { step_running_ = 0; @@ -245,24 +262,22 @@ namespace Modelec void ActionExecutor::ToggleArm(BaseAction::Front front, bool force) { - if (action_done_ || force) + if (arm_pos_[front].down) { - if (arm_pos_[front].down) - { - Up(front, force); - } - else - { - Down(front, force); - } + Up(front, force); + } + else + { + Down(front, force); } } void ActionExecutor::RotateArm(BaseAction::Front front, bool force, bool rotated) { - if ((action_done_ && arm_pos_[front].rotated != rotated) || force) + if (arm_pos_[front].rotated != rotated || force) { - action_ = std::make_shared(shared_from_this(), front, rotated); + auto action = std::make_shared(shared_from_this(), front, rotated); + action_.push(action); if (action_done_) { step_running_ = 0; @@ -275,63 +290,57 @@ namespace Modelec } } - void ActionExecutor::Take(const std::vector>& servos, bool force) { - if (action_done_ || force) + void ActionExecutor::Take(const std::vector>& servos) { + auto action = std::make_shared(shared_from_this(), servos); + action_.push(action); + if (action_done_) { - action_ = std::make_shared(shared_from_this(), servos); - if (action_done_) - { - step_running_ = 0; - } - action_done_ = false; - + step_running_ = 0; + } + action_done_ = false; - for (auto servo : servos) - { - servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = true; - } - Update(); + for (auto servo : servos) + { + servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = true; } + + Update(); } - void ActionExecutor::Free(const std::vector>& servos, bool force) { - if (action_done_ || force) + void ActionExecutor::Free(const std::vector>& servos) { + auto action = std::make_shared(shared_from_this(), servos); + action_.push(action); + if (action_done_) { - action_ = std::make_shared(shared_from_this(), servos); - if (action_done_) - { - step_running_ = 0; - } - action_done_ = false; - - for (auto servo : servos) - { - servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = false; - } + step_running_ = 0; + } + action_done_ = false; - Update(); + for (auto servo : servos) + { + servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = false; } + + Update(); } - void ActionExecutor::ToggleServo(const std::vector>& servos, bool force) + void ActionExecutor::ToggleServo(const std::vector>& servos) { - if (action_done_ || force) + auto action = std::make_shared(shared_from_this(), servos); + action_.push(action); + if (action_done_) { - action_ = std::make_shared(shared_from_this(), servos); - if (action_done_) - { - step_running_ = 0; - } - action_done_ = false; - - for (auto servo : servos) - { - servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = !servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)]; - } + step_running_ = 0; + } + action_done_ = false; - Update(); + for (auto servo : servos) + { + servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = !servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)]; } + + Update(); } void ActionExecutor::MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index c981a8f..da62b2d 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -146,7 +146,7 @@ namespace Modelec action_executor_->Free({ {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}, {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, - }, true); + }); Transition(State::WAIT_START, "System ready"); } From 58eaeaac1b475fb921f6d74f76ac4a1c2d24c68c Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 17:52:45 +0100 Subject: [PATCH 12/81] action --- src/modelec_strat/src/action_executor.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 0d3922f..d9bf2a7 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -129,7 +129,10 @@ namespace Modelec if (left_trig == 1 && last_left_trig == -1) // left trigger pressed { - ToggleArm(BaseAction::BACK); + if (action_done_) + { + ToggleArm(BaseAction::BACK); + } last_left_trig = left_trig; } else if (left_trig == -1 && last_left_trig == 1) // left trigger released { @@ -138,7 +141,10 @@ namespace Modelec if (right_trig == 1 && last_left_trig == -1) // right trigger pressed { - ToggleArm(BaseAction::FRONT); + if (action_done_) + { + ToggleArm(BaseAction::FRONT); + } last_right_trig = right_trig; } else if (right_trig == -1 && last_right_trig == 1) // right trigger released { From 3ab81f10c6d125a28aff5f705c0b4f143e4c4b97 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 17:54:19 +0100 Subject: [PATCH 13/81] action --- src/modelec_strat/src/action_executor.cpp | 51 ++++++++++++++++++----- 1 file changed, 40 insertions(+), 11 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index d9bf2a7..aaf15b1 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -91,27 +91,45 @@ namespace Modelec { if (msg->buttons[0] == 1) // A button { - ToggleServo({{0, BaseAction::FRONT}}); + if (action_done_) + { + ToggleServo({{0, BaseAction::FRONT}}); + } } else if (msg->buttons[1] == 1) // B button { - ToggleServo({{1, BaseAction::FRONT}}); + if (action_done_) + { + ToggleServo({{1, BaseAction::FRONT}}); + } } else if (msg->buttons[3] == 1) // X button { - ToggleServo({{3, BaseAction::FRONT}}); + if (action_done_) + { + ToggleServo({{3, BaseAction::FRONT}}); + } } else if (msg->buttons[4] == 1) // Y button { - ToggleServo({{2, BaseAction::FRONT}}); + if (action_done_) + { + ToggleServo({{2, BaseAction::FRONT}}); + } } else if (msg->buttons[6] == 1) // L1 button { - RotateArm(BaseAction::BACK, false, !arm_pos_[BaseAction::BACK].rotated); + if (action_done_) + { + RotateArm(BaseAction::BACK, false, !arm_pos_[BaseAction::BACK].rotated); + } } else if (msg->buttons[7] == 1) // R1 button { - RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated); + if (action_done_) + { + RotateArm(BaseAction::FRONT, false, !arm_pos_[BaseAction::FRONT].rotated); + } } else if (msg->buttons[14] == 1) // LT button { @@ -156,20 +174,31 @@ namespace Modelec if (btn_horizontal == 1.0) // left { - ToggleServo({{3, BaseAction::BACK}}); + if (action_done_) + { + ToggleServo({{3, BaseAction::BACK}}); + } } else if (btn_horizontal == -1.0) // right { - ToggleServo({{1, BaseAction::BACK}}); + if (action_done_) + { + ToggleServo({{1, BaseAction::BACK}}); + } } - if (btn_vertical == 1.0) // up { - ToggleServo({{0, BaseAction::BACK}}); + if (action_done_) + { + ToggleServo({{0, BaseAction::BACK}}); + } } else if (btn_vertical == -1.0) // down { - ToggleServo({{2, BaseAction::BACK}}); + if (action_done_) + { + ToggleServo({{2, BaseAction::BACK}}); + } } } }); From 2a503f17dda8fe69e74604137f9bf923d3e58701 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 18:30:37 +0100 Subject: [PATCH 14/81] log --- src/modelec_strat/src/action_executor.cpp | 35 ++++++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index aaf15b1..e9ed0d8 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -262,6 +262,11 @@ namespace Modelec } void ActionExecutor::Down(BaseAction::Front front, bool force) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor Down called for front=%d, force=%d", + static_cast(front), + static_cast(force)); if ((!arm_pos_[front].down) || force) { auto action = std::make_shared(shared_from_this(), front); @@ -279,6 +284,11 @@ namespace Modelec } void ActionExecutor::Up(BaseAction::Front front, bool force) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor Up called for front=%d, force=%d", + static_cast(front), + static_cast(force)); if ((arm_pos_[front].down) || force) { auto action = std::make_shared(shared_from_this(), front); @@ -297,6 +307,11 @@ namespace Modelec void ActionExecutor::ToggleArm(BaseAction::Front front, bool force) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor ToggleArm called for front=%d, force=%d", + static_cast(front), + static_cast(force)); if (arm_pos_[front].down) { Up(front, force); @@ -309,6 +324,12 @@ namespace Modelec void ActionExecutor::RotateArm(BaseAction::Front front, bool force, bool rotated) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor RotateArm called for front=%d, force=%d, rotated=%d", + static_cast(front), + static_cast(force), + static_cast(rotated)); if (arm_pos_[front].rotated != rotated || force) { auto action = std::make_shared(shared_from_this(), front, rotated); @@ -326,6 +347,10 @@ namespace Modelec } void ActionExecutor::Take(const std::vector>& servos) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor Take called for %d servos", + static_cast(servos.size())); auto action = std::make_shared(shared_from_this(), servos); action_.push(action); if (action_done_) @@ -344,6 +369,10 @@ namespace Modelec } void ActionExecutor::Free(const std::vector>& servos) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor Free called for %d servos", + static_cast(servos.size())); auto action = std::make_shared(shared_from_this(), servos); action_.push(action); if (action_done_) @@ -362,6 +391,10 @@ namespace Modelec void ActionExecutor::ToggleServo(const std::vector>& servos) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor ToggleServo called for %d servos", + static_cast(servos.size())); auto action = std::make_shared(shared_from_this(), servos); action_.push(action); if (action_done_) @@ -383,7 +416,7 @@ namespace Modelec servo_timed_move_pub_->publish(msg); step_running_ += msg.items.size(); - RCLCPP_DEBUG( + RCLCPP_INFO( node_->get_logger(), "ActionExecutor MoveServoTimed called with %d items, step_running_=%d", static_cast(msg.items.size()), From fe61d95340db179bcc721c1963304b5d206e7806 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 18:40:05 +0100 Subject: [PATCH 15/81] enemy manager --- no_lidar.joy.ros2_launch_marcel.desktop | 2 +- no_lidar.ros2_launch_marcel.desktop | 4 +- src/modelec_core/launch/modelec.launch.py | 170 ++++++++++++++-------- 3 files changed, 111 insertions(+), 65 deletions(-) diff --git a/no_lidar.joy.ros2_launch_marcel.desktop b/no_lidar.joy.ros2_launch_marcel.desktop index e323a0d..34ba877 100644 --- a/no_lidar.joy.ros2_launch_marcel.desktop +++ b/no_lidar.joy.ros2_launch_marcel.desktop @@ -2,7 +2,7 @@ Type=Application Name=Joy no Lidar Comment=Lance le système ROS 2 avec GUI -Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_enemy_manager:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/no_lidar.ros2_launch_marcel.desktop b/no_lidar.ros2_launch_marcel.desktop index ff3a9d2..2847faf 100644 --- a/no_lidar.ros2_launch_marcel.desktop +++ b/no_lidar.ros2_launch_marcel.desktop @@ -2,9 +2,7 @@ Type=Application Name=No Lidar Comment=Lance le système ROS 2 avec GUI - -Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false - +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false with_enemy_manager:=false Icon=utilities-terminal Terminal=true Categories=Utility; diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 7269c7a..fbc7dbb 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,31 +1,54 @@ from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown, RegisterEventHandler, TimerAction, LogInfo +from launch.actions import ( + DeclareLaunchArgument, + OpaqueFunction, + Shutdown, + RegisterEventHandler, + TimerAction, + LogInfo, +) from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): - # Declare launch arguments - with_gui_arg = DeclareLaunchArgument('with_gui', default_value='true', description='Launch GUI?') - with_rplidar_arg = DeclareLaunchArgument('with_rplidar', default_value='true', description='Launch RPLIDAR?') - with_com_arg = DeclareLaunchArgument('with_com', default_value='true', description='Launch COM nodes?') - with_strat_arg = DeclareLaunchArgument('with_strat', default_value='true', description='Launch strategy nodes?') - with_joy_arg = DeclareLaunchArgument('with_joy', default_value='true', description='Launch joystick node?') + # ------------------------------------------------- + # Launch arguments + # ------------------------------------------------- + with_gui_arg = DeclareLaunchArgument( + 'with_gui', default_value='true', description='Launch GUI?' + ) + with_rplidar_arg = DeclareLaunchArgument( + 'with_rplidar', default_value='true', description='Launch RPLIDAR?' + ) + with_com_arg = DeclareLaunchArgument( + 'with_com', default_value='true', description='Launch COM nodes?' + ) + with_strat_arg = DeclareLaunchArgument( + 'with_strat', default_value='true', description='Launch strategy nodes (except enemy_manager)?' + ) + with_enemy_manager_arg = DeclareLaunchArgument( + 'with_enemy_manager', + default_value='true', + description='Launch enemy_manager node?', + ) + with_joy_arg = DeclareLaunchArgument( + 'with_joy', default_value='true', description='Launch joystick node?' + ) channel_type = LaunchConfiguration('channel_type', default='serial') serial_port = LaunchConfiguration('serial_port', default='/dev/rplidar') serial_baudrate = LaunchConfiguration('serial_baudrate', default='115200') frame_id = LaunchConfiguration('frame_id', default='laser') inverted = LaunchConfiguration('inverted', default='false') - angle_compensate = LaunchConfiguration('angle_compensate', default='true') + angle_compensate = LaunchConfiguration('angle_compensate', default='false') scan_mode = LaunchConfiguration('scan_mode', default='Standard') - # ---------------------------- - # Infinite LIDAR restart logic - # ---------------------------- + # ------------------------------------------------- + # RPLIDAR with auto-restart + # ------------------------------------------------- def create_lidar_with_restart(): - """Create a lidar node and its auto-restart handler.""" lidar_node = Node( package='rplidar_ros', executable='rplidar_node', @@ -39,10 +62,9 @@ def create_lidar_with_restart(): 'angle_compensate': angle_compensate, 'scan_mode': scan_mode, }], - output='screen' + output='screen', ) - # Instead of recursion at definition time, we delay the re-creation using a lambda restart_handler = RegisterEventHandler( OnProcessExit( target_action=lidar_node, @@ -50,9 +72,13 @@ def create_lidar_with_restart(): LogInfo(msg='[Launch] RPLIDAR crashed — restarting in 2s...'), TimerAction( period=2.0, - actions=[OpaqueFunction(function=lambda *_: create_lidar_with_restart())] - ) - ] + actions=[ + OpaqueFunction( + function=lambda *_: create_lidar_with_restart() + ) + ], + ), + ], ) ) @@ -60,37 +86,37 @@ def create_lidar_with_restart(): def launch_rplidar_restart_if_needed(context, *args, **kwargs): if context.launch_configurations.get('with_rplidar') == 'true': - # Start after 3 seconds return [ TimerAction( period=3.0, - actions=create_lidar_with_restart() + actions=create_lidar_with_restart(), ) ] return [] - # ---------------------------- - # GUI node - # ---------------------------- + # ------------------------------------------------- + # GUI + # ------------------------------------------------- def launch_gui(context, *args, **kwargs): if context.launch_configurations.get('with_gui') == 'true': gui = Node( package='modelec_gui', executable='modelec_gui', - name='modelec_gui' + name='modelec_gui', ) + shutdown = RegisterEventHandler( OnProcessExit( target_action=gui, - on_exit=[Shutdown()] + on_exit=[Shutdown()], ) ) return [gui, shutdown] return [] - # ---------------------------- + # ------------------------------------------------- # COM nodes - # ---------------------------- + # ------------------------------------------------- def launch_com(context, *args, **kwargs): if context.launch_configurations.get('with_com') == 'true': return [ @@ -99,73 +125,95 @@ def launch_com(context, *args, **kwargs): executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/dev/USB_ODO", + 'serial_port': '/dev/USB_ODO', 'baudrate': 115200, - 'name': "pcb_odo", - }] + 'name': 'pcb_odo', + }], ), Node( package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/dev/USB_ACTION", + 'serial_port': '/dev/USB_ACTION', 'baudrate': 115200, - 'name': "pcb_action", - }] + 'name': 'pcb_action', + }], ), ] return [] - # ---------------------------- - # Strategy nodes - # ---------------------------- + # ------------------------------------------------- + # Strategy nodes (WITHOUT enemy_manager) + # ------------------------------------------------- def launch_strat(context, *args, **kwargs): if context.launch_configurations.get('with_strat') == 'true': return [ - Node(package='modelec_strat', executable='strat_fsm', name='strat_fsm'), - Node(package='modelec_strat', executable='pami_manager', name='pami_manager'), - Node(package='modelec_strat', executable='enemy_manager', name='enemy_manager'), + Node( + package='modelec_strat', + executable='strat_fsm', + name='strat_fsm', + ), + Node( + package='modelec_strat', + executable='pami_manager', + name='pami_manager', + ), ] return [] - # ---------------------------- - # Joy node - # ---------------------------- + # ------------------------------------------------- + # Enemy manager (standalone) + # ------------------------------------------------- + def launch_enemy_manager(context, *args, **kwargs): + if context.launch_configurations.get('with_enemy_manager') == 'true': + return [ + Node( + package='modelec_strat', + executable='enemy_manager', + name='enemy_manager', + ) + ] + return [] + + # ------------------------------------------------- + # Joystick + # ------------------------------------------------- def launch_joy(context, *args, **kwargs): - if context.launch_configurations.get('with_joy', 'true') == 'true': - joy = Node( - package='joy', - executable='joy_node', - name='joy_node', - output='screen' - ) - return [joy] + if context.launch_configurations.get('with_joy') == 'true': + return [ + Node( + package='joy', + executable='joy_node', + name='joy_node', + output='screen', + ) + ] return [] - # ---------------------------- - # Final launch description - # ---------------------------- + # ------------------------------------------------- + # Final LaunchDescription + # ------------------------------------------------- return LaunchDescription([ - DeclareLaunchArgument('channel_type', default_value=channel_type, description='Specifying channel type of lidar'), - DeclareLaunchArgument('serial_port', default_value=serial_port, description='Specifying usb port to connected lidar'), - DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate, description='Specifying usb port baudrate to connected lidar'), - DeclareLaunchArgument('frame_id', default_value=frame_id, description='Specifying frame_id of lidar'), - DeclareLaunchArgument('inverted', default_value=inverted, description='Specifying whether or not to invert scan data'), - DeclareLaunchArgument('angle_compensate', default_value=angle_compensate, description='Specifying whether or not to enable angle_compensate of scan data'), - DeclareLaunchArgument('scan_mode', default_value=scan_mode, description='Specifying scan mode of lidar'), + DeclareLaunchArgument('channel_type', default_value=channel_type), + DeclareLaunchArgument('serial_port', default_value=serial_port), + DeclareLaunchArgument('serial_baudrate', default_value=serial_baudrate), + DeclareLaunchArgument('frame_id', default_value=frame_id), + DeclareLaunchArgument('inverted', default_value=inverted), + DeclareLaunchArgument('angle_compensate', default_value=angle_compensate), + DeclareLaunchArgument('scan_mode', default_value=scan_mode), with_gui_arg, with_rplidar_arg, with_com_arg, with_strat_arg, + with_enemy_manager_arg, with_joy_arg, OpaqueFunction(function=launch_rplidar_restart_if_needed), OpaqueFunction(function=launch_gui), OpaqueFunction(function=launch_com), OpaqueFunction(function=launch_strat), + OpaqueFunction(function=launch_enemy_manager), OpaqueFunction(function=launch_joy), ]) - -# to run in debug : , prefix=['xterm -e gdb -ex run --args'] From 6a4fa8988a72509c4b2d56ae008ad50895f87417 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 18:49:27 +0100 Subject: [PATCH 16/81] log --- src/modelec_strat/src/action_executor.cpp | 39 +++++++++++++---------- 1 file changed, 22 insertions(+), 17 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index e9ed0d8..779e7ff 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -262,13 +262,14 @@ namespace Modelec } void ActionExecutor::Down(BaseAction::Front front, bool force) { - RCLCPP_INFO( - node_->get_logger(), - "ActionExecutor Down called for front=%d, force=%d", - static_cast(front), - static_cast(force)); - if ((!arm_pos_[front].down) || force) + if (!arm_pos_[front].down || force) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor Down called for front=%d, force=%d", + static_cast(front), + static_cast(force)); + auto action = std::make_shared(shared_from_this(), front); action_.push(action); if (action_done_) @@ -284,13 +285,14 @@ namespace Modelec } void ActionExecutor::Up(BaseAction::Front front, bool force) { - RCLCPP_INFO( - node_->get_logger(), - "ActionExecutor Up called for front=%d, force=%d", - static_cast(front), - static_cast(force)); if ((arm_pos_[front].down) || force) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor Up called for front=%d, force=%d", + static_cast(front), + static_cast(force)); + auto action = std::make_shared(shared_from_this(), front); action_.push(action); if (action_done_) @@ -324,14 +326,15 @@ namespace Modelec void ActionExecutor::RotateArm(BaseAction::Front front, bool force, bool rotated) { - RCLCPP_INFO( - node_->get_logger(), - "ActionExecutor RotateArm called for front=%d, force=%d, rotated=%d", - static_cast(front), - static_cast(force), - static_cast(rotated)); if (arm_pos_[front].rotated != rotated || force) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor RotateArm called for front=%d, force=%d, rotated=%d", + static_cast(front), + static_cast(force), + static_cast(rotated)); + auto action = std::make_shared(shared_from_this(), front, rotated); action_.push(action); if (action_done_) @@ -351,6 +354,7 @@ namespace Modelec node_->get_logger(), "ActionExecutor Take called for %d servos", static_cast(servos.size())); + auto action = std::make_shared(shared_from_this(), servos); action_.push(action); if (action_done_) @@ -373,6 +377,7 @@ namespace Modelec node_->get_logger(), "ActionExecutor Free called for %d servos", static_cast(servos.size())); + auto action = std::make_shared(shared_from_this(), servos); action_.push(action); if (action_done_) From 72557bf259d6889e95ad99ca0b9491e54c6b5ebf Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 18:54:01 +0100 Subject: [PATCH 17/81] fuck me --- src/modelec_strat/src/action_executor.cpp | 38 +++++++++++++++++++++-- 1 file changed, 35 insertions(+), 3 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 779e7ff..0a426d4 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -278,7 +278,15 @@ namespace Modelec } action_done_ = false; - arm_pos_[front].down = true; + if (front == BaseAction::BOTH) + { + arm_pos_[BaseAction::FRONT].down = true; + arm_pos_[BaseAction::BACK].down = true; + } + else + { + arm_pos_[front].down = true; + } Update(); } @@ -301,7 +309,15 @@ namespace Modelec } action_done_ = false; - arm_pos_[front].down = false; + if (front == BaseAction::BOTH) + { + arm_pos_[BaseAction::FRONT].down = false; + arm_pos_[BaseAction::BACK].down = false; + } + else + { + arm_pos_[front].down = false; + } Update(); } @@ -314,6 +330,14 @@ namespace Modelec "ActionExecutor ToggleArm called for front=%d, force=%d", static_cast(front), static_cast(force)); + + if (front == BaseAction::BOTH) + { + ToggleArm(BaseAction::FRONT, force); + ToggleArm(BaseAction::BACK, force); + return; + } + if (arm_pos_[front].down) { Up(front, force); @@ -343,7 +367,15 @@ namespace Modelec } action_done_ = false; - arm_pos_[front].rotated = rotated; + if (front == BaseAction::BOTH) + { + arm_pos_[BaseAction::FRONT].rotated = rotated; + arm_pos_[BaseAction::BACK].rotated = rotated; + } + else + { + arm_pos_[front].rotated = rotated; + } Update(); } From e9e46806e3115fb8ed8d0861b3e5109a84b4fd1d Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:03:14 +0100 Subject: [PATCH 18/81] const + PID --- src/modelec_utils/include/modelec_utils/point.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 90850ac..ddc4c05 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -1,7 +1,7 @@ #pragma once -#define CLOSE_DISTANCE 165 -#define BASE_DISTANCE 290 +#define CLOSE_DISTANCE 155 +#define BASE_DISTANCE 310 namespace Modelec { From 45fa33a6bab1c30dff35ca1df70268aeb68b034c Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:03:32 +0100 Subject: [PATCH 19/81] const + PID --- src/modelec_com/src/pcb_odo_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 8c79f74..560a9eb 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -112,7 +112,7 @@ namespace Modelec this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); SetPID("THETA", 14, 0, 0); - SetPID("POS", 10, 0, 0); + SetPID("POS", 5, 0, 0); SetPID("LEFT", 5, 0, 0); SetPID("RIGHT", 5, 0, 0); } From b9df6ddcdab92771f7cd83ec8bf7fb499ff1a381 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:11:49 +0100 Subject: [PATCH 20/81] config --- src/modelec_strat/data/config.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 307e602..da8cd89 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -41,10 +41,10 @@ - + - + 450 450 From 2d521e92601ffc288134cb5a9ea835f2d0ef885d Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:15:48 +0100 Subject: [PATCH 21/81] config --- src/modelec_strat/data/config.xml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index da8cd89..41ac8a2 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -36,15 +36,15 @@ - - + + - + - + 450 450 From cb731406b2bf564a1dd489ac1fee8ce9112f7876 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:22:06 +0100 Subject: [PATCH 22/81] reset game action queue --- src/modelec_strat/src/strat_fms.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index da62b2d..8ddedbb 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -66,8 +66,6 @@ namespace Modelec RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); } - game_action_sequence_.push(State::TAKE_MISSION); - game_action_sequence_.push(State::FREE_MISSION); static_strat_ = Config::get("config.static_strat", false); } @@ -148,6 +146,11 @@ namespace Modelec {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, }); + auto empty_queue_ = std::queue(); + std::swap(game_action_sequence_, empty_queue_); + game_action_sequence_.push(State::TAKE_MISSION); + game_action_sequence_.push(State::FREE_MISSION); + Transition(State::WAIT_START, "System ready"); } break; From cf226774526c5521a12728007718bdca211770ff Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:42:34 +0100 Subject: [PATCH 23/81] log --- src/modelec_strat/src/action_executor.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 0a426d4..53ac75a 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -264,7 +264,7 @@ namespace Modelec void ActionExecutor::Down(BaseAction::Front front, bool force) { if (!arm_pos_[front].down || force) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Down called for front=%d, force=%d", static_cast(front), @@ -295,7 +295,7 @@ namespace Modelec void ActionExecutor::Up(BaseAction::Front front, bool force) { if ((arm_pos_[front].down) || force) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Up called for front=%d, force=%d", static_cast(front), @@ -325,7 +325,7 @@ namespace Modelec void ActionExecutor::ToggleArm(BaseAction::Front front, bool force) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor ToggleArm called for front=%d, force=%d", static_cast(front), @@ -352,7 +352,7 @@ namespace Modelec { if (arm_pos_[front].rotated != rotated || force) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor RotateArm called for front=%d, force=%d, rotated=%d", static_cast(front), @@ -382,7 +382,7 @@ namespace Modelec } void ActionExecutor::Take(const std::vector>& servos) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Take called for %d servos", static_cast(servos.size())); @@ -405,7 +405,7 @@ namespace Modelec } void ActionExecutor::Free(const std::vector>& servos) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Free called for %d servos", static_cast(servos.size())); @@ -428,10 +428,11 @@ namespace Modelec void ActionExecutor::ToggleServo(const std::vector>& servos) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor ToggleServo called for %d servos", static_cast(servos.size())); + auto action = std::make_shared(shared_from_this(), servos); action_.push(action); if (action_done_) @@ -453,7 +454,7 @@ namespace Modelec servo_timed_move_pub_->publish(msg); step_running_ += msg.items.size(); - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor MoveServoTimed called with %d items, step_running_=%d", static_cast(msg.items.size()), From fd99d2ef0169ceabc1f5f9c019d852af42af70e1 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 29 Jan 2026 19:51:32 +0100 Subject: [PATCH 24/81] pb waypoints --- src/modelec_com/src/pcb_odo_interface.cpp | 2 +- src/modelec_strat/src/navigation_helper.cpp | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 560a9eb..cf6b8b7 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -179,7 +179,7 @@ namespace Modelec { if (tokens[2] == "REACH") { - // RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); + RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); int id = std::stoi(tokens[3]); diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index c219c29..2607f02 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -722,6 +722,7 @@ namespace Modelec void NavigationHelper::AskWaypoint() { + RCLCPP_INFO(node_->get_logger(), "Asking for active waypoint..."); std_msgs::msg::Empty msg; odo_ask_waypoint_pub_->publish(msg); } From c890b161d6a91124ac816e4fd0328ac8e3afef9f Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 13:16:43 +0100 Subject: [PATCH 25/81] servo value --- src/modelec_strat/src/action/down_action.cpp | 16 ++++++++-------- src/modelec_strat/src/action/up_action.cpp | 18 +++++++++--------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index fe23e92..ba55585 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -34,24 +34,24 @@ void Modelec::DownAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 1.75; - msg.items[0].end_angle = 3; + msg.items[0].start_angle = 1.76; + msg.items[0].end_angle = 2.98; msg.items[0].duration_s = 1; msg.items[1].id = 1; - msg.items[1].start_angle = 1.9; - msg.items[1].end_angle = 0.85; + msg.items[1].start_angle = 2.06; + msg.items[1].end_angle = 0.9; msg.items[1].duration_s = 1; msg.items[2].id = 2; - msg.items[2].start_angle = 0.2; + msg.items[2].start_angle = 2.9; msg.items[2].end_angle = 0; - msg.items[2].duration_s = 0.2; + msg.items[2].duration_s = 0.5; msg.items[3].id = 3; - msg.items[3].start_angle = 2.8; + msg.items[3].start_angle = 0; msg.items[3].end_angle = 2.9; - msg.items[3].duration_s = 0.2; + msg.items[3].duration_s = 0.5; } if (front_ == BACK || front_ == BOTH) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index ded689e..f41c58b 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -34,24 +34,24 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 3; - msg.items[0].end_angle = 1.75; + msg.items[0].start_angle = 2.98; + msg.items[0].end_angle = 1.76; msg.items[0].duration_s = 1; msg.items[1].id = 1; - msg.items[1].start_angle = 0.85; - msg.items[1].end_angle = 1.9; + msg.items[1].start_angle = 0.9; + msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 1; msg.items[2].id = 2; - msg.items[2].start_angle = 0.1; - msg.items[2].end_angle = 0.2; - msg.items[2].duration_s = 1; + msg.items[2].start_angle = 0; + msg.items[2].end_angle = 2.9; + msg.items[2].duration_s = 0.5; msg.items[3].id = 3; msg.items[3].start_angle = 2.9; - msg.items[3].end_angle = 2.8; - msg.items[3].duration_s = 1; + msg.items[3].end_angle = 0; + msg.items[3].duration_s = 0.5; } if (front_ == BACK || front_ == BOTH) { From 8243bd39e100030f5a074b75f45c91c7329bb028 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 13:19:08 +0100 Subject: [PATCH 26/81] servo value --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index ba55585..37bb24a 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -36,12 +36,12 @@ void Modelec::DownAction::Next() msg.items[0].id = 0; msg.items[0].start_angle = 1.76; msg.items[0].end_angle = 2.98; - msg.items[0].duration_s = 1; + msg.items[0].duration_s = 2; msg.items[1].id = 1; msg.items[1].start_angle = 2.06; msg.items[1].end_angle = 0.9; - msg.items[1].duration_s = 1; + msg.items[1].duration_s = 2; msg.items[2].id = 2; msg.items[2].start_angle = 2.9; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index f41c58b..ac31eed 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -36,12 +36,12 @@ void Modelec::UPAction::Next() msg.items[0].id = 0; msg.items[0].start_angle = 2.98; msg.items[0].end_angle = 1.76; - msg.items[0].duration_s = 1; + msg.items[0].duration_s = 2; msg.items[1].id = 1; msg.items[1].start_angle = 0.9; msg.items[1].end_angle = 2.06; - msg.items[1].duration_s = 1; + msg.items[1].duration_s = 2; msg.items[2].id = 2; msg.items[2].start_angle = 0; From 93acaa632db47c3fb2332957c734d80f70b3d29a Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 13:21:47 +0100 Subject: [PATCH 27/81] servo value --- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index ac31eed..fe41e56 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -46,12 +46,12 @@ void Modelec::UPAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 0; msg.items[2].end_angle = 2.9; - msg.items[2].duration_s = 0.5; + msg.items[2].duration_s = 2; msg.items[3].id = 3; msg.items[3].start_angle = 2.9; msg.items[3].end_angle = 0; - msg.items[3].duration_s = 0.5; + msg.items[3].duration_s = 2; } if (front_ == BACK || front_ == BOTH) { From 4ffab61555f49efe32f1250dcb4f274a26396136 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 13:23:28 +0100 Subject: [PATCH 28/81] servo value --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 37bb24a..8058f3a 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -46,12 +46,12 @@ void Modelec::DownAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 2.9; msg.items[2].end_angle = 0; - msg.items[2].duration_s = 0.5; + msg.items[2].duration_s = 1; msg.items[3].id = 3; msg.items[3].start_angle = 0; msg.items[3].end_angle = 2.9; - msg.items[3].duration_s = 0.5; + msg.items[3].duration_s = 1; } if (front_ == BACK || front_ == BOTH) From 49a676f0fe6095f13ba895a803af957e65d7fdcd Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:00:31 +0100 Subject: [PATCH 29/81] servo value --- src/modelec_strat/src/action/down_action.cpp | 12 ++++++------ src/modelec_strat/src/action/up_action.cpp | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 8058f3a..3889e1d 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -35,22 +35,22 @@ void Modelec::DownAction::Next() { msg.items[0].id = 0; msg.items[0].start_angle = 1.76; - msg.items[0].end_angle = 2.98; + msg.items[0].end_angle = 2.91; msg.items[0].duration_s = 2; msg.items[1].id = 1; msg.items[1].start_angle = 2.06; - msg.items[1].end_angle = 0.9; + msg.items[1].end_angle = 0.95; msg.items[1].duration_s = 2; msg.items[2].id = 2; - msg.items[2].start_angle = 2.9; - msg.items[2].end_angle = 0; + msg.items[2].start_angle = 0.5; + msg.items[2].end_angle = 3.2; msg.items[2].duration_s = 1; msg.items[3].id = 3; - msg.items[3].start_angle = 0; - msg.items[3].end_angle = 2.9; + msg.items[3].start_angle = 2.6; + msg.items[3].end_angle = 0; msg.items[3].duration_s = 1; } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index fe41e56..e831452 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -34,23 +34,23 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 2.98; + msg.items[0].start_angle = 2.91; msg.items[0].end_angle = 1.76; msg.items[0].duration_s = 2; msg.items[1].id = 1; - msg.items[1].start_angle = 0.9; + msg.items[1].start_angle = 0.95; msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 2; msg.items[2].id = 2; - msg.items[2].start_angle = 0; - msg.items[2].end_angle = 2.9; + msg.items[2].start_angle = 3.2; + msg.items[2].end_angle = 0.5; msg.items[2].duration_s = 2; msg.items[3].id = 3; - msg.items[3].start_angle = 2.9; - msg.items[3].end_angle = 0; + msg.items[3].start_angle = 0; + msg.items[3].end_angle = 2.6; msg.items[3].duration_s = 2; } From fd40f30869e37a2ef7e37ebc8589818d62969d10 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:11:05 +0100 Subject: [PATCH 30/81] servo value --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 3889e1d..3d7cbf6 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -46,12 +46,12 @@ void Modelec::DownAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 0.5; msg.items[2].end_angle = 3.2; - msg.items[2].duration_s = 1; + msg.items[2].duration_s = 2; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; msg.items[3].end_angle = 0; - msg.items[3].duration_s = 1; + msg.items[3].duration_s = 2; } if (front_ == BACK || front_ == BOTH) From 9f70eff0629d1f312daa2337eb0ac22cd14cd939 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:21:13 +0100 Subject: [PATCH 31/81] first setup --- src/modelec_com/src/pcb_action_interface.cpp | 20 +++++++++---------- .../modelec_strat/action/down_action.hpp | 5 ++++- .../include/modelec_strat/action_executor.hpp | 2 ++ src/modelec_strat/src/action/down_action.cpp | 5 +++-- src/modelec_strat/src/action_executor.cpp | 10 +++++----- 5 files changed, 24 insertions(+), 18 deletions(-) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 0c3ce6d..4064dc4 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -262,15 +262,15 @@ namespace Modelec {5, 0} };*/ - /*servo_value_ = { - {0, 2.95}, - {1, 0.93}, - {2, 0}, - {3, 3}, - {4, 0.8}, - {5, 0.8}, - {6, 0.8}, - {7, 0.8}, + servo_value_ = { + {0, 2.91}, + {1, 0.95}, + {2, 3.2}, + {3, 0}, + {4, 1}, + {5, 1}, + {6, 1}, + {7, 1}, }; std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";"; @@ -282,7 +282,7 @@ namespace Modelec data += "\n"; - SendToPCB(data);*/ + SendToPCB(data); /*relay_value_ = { {1, false}, diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index 814e01e..3eda570 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -9,17 +9,20 @@ namespace Modelec { public: DownAction(const std::shared_ptr& action_executor); - DownAction(const std::shared_ptr& action_executor, Front front); + DownAction(const std::shared_ptr& action_executor, Front front, bool inverted); void Next() override; void Init(const std::vector& params) override; void SetFront(Front front); + void SetInverted(bool inverted); inline static const std::string Name = ActionExec::DOWN; private: Front front_; + bool inverted_; + std::queue steps_; inline static const bool registered_ = diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 07436b4..9b00b8f 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -60,6 +60,8 @@ namespace Modelec }; std::array arm_pos_; + std::array servo_value_; + bool IsEmpty() const; bool IsFull() const; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 3d7cbf6..5be3fe1 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -2,15 +2,16 @@ #include "modelec_strat/action_executor.hpp" -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor), front_(BOTH), inverted_(false) { steps_.push(ActionExec::DOWN_STEP); steps_.push(ActionExec::DONE_STEP); } -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Front front) : DownAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Front front, bool inverted) : DownAction(action_executor) { front_ = front; + inverted_ = inverted; } void Modelec::DownAction::Next() diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 53ac75a..5587c84 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -28,12 +28,12 @@ namespace Modelec }); servo_move_res_sub_ = node_->create_subscription( - "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr) + "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg) { - // BUG - // if ServoTimed is called this one will trigger so step_running_ will be decremented at the beginning of the Timed one - // step_running_ -= msg->items.size(); - // Update(); + for (const auto& item : msg->items) + { + servo_value_[item.id] = item.angle; + } }); relay_move_res_sub_ = node_->create_subscription( From e8e16fb4e061df5a1c447c0aa2f5c4994d8cd2b4 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:27:52 +0100 Subject: [PATCH 32/81] maybe ?? --- src/modelec_strat/CMakeLists.txt | 1 - .../modelec_strat/action/down_action.hpp | 2 +- .../action/rotate_arm_action.hpp | 39 -------- src/modelec_strat/src/action/down_action.cpp | 18 ++-- src/modelec_strat/src/action/free_action.cpp | 2 +- .../src/action/rotate_arm_action.cpp | 92 ------------------- src/modelec_strat/src/action/take_action.cpp | 2 +- src/modelec_strat/src/action/up_action.cpp | 8 +- src/modelec_strat/src/action_executor.cpp | 8 +- 9 files changed, 26 insertions(+), 146 deletions(-) delete mode 100644 src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp delete mode 100644 src/modelec_strat/src/action/rotate_arm_action.cpp diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 3d67b68..bd2cdfb 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -26,7 +26,6 @@ set(strat_fsm_sources src/action/free_action.cpp src/action/take_action.cpp src/action/toggle_servo_action.cpp - src/action/rotate_arm_action.cpp src/missions/go_home_mission.cpp src/missions/take_mission.cpp diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index 3eda570..4a64957 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -9,7 +9,7 @@ namespace Modelec { public: DownAction(const std::shared_ptr& action_executor); - DownAction(const std::shared_ptr& action_executor, Front front, bool inverted); + DownAction(const std::shared_ptr& action_executor, Front front, bool inverted = false); void Next() override; void Init(const std::vector& params) override; diff --git a/src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp b/src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp deleted file mode 100644 index 26b6f47..0000000 --- a/src/modelec_strat/include/modelec_strat/action/rotate_arm_action.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include - -namespace Modelec -{ - class RotateArmAction : public BaseAction - { - public: - RotateArmAction(const std::shared_ptr& action_executor); - RotateArmAction(const std::shared_ptr& action_executor, Front front, bool rotated = false); - - void Next() override; - void Init(const std::vector& params) override; - void SetFront(Front front); - void SetRotated(bool rotated); - - inline static const std::string Name = ActionExec::TOGGLE_ARM; - - private: - Front front_; - - bool rotated_ = false; - - std::queue steps_; - - inline static const bool registered_ = - []() - { - BaseAction::Registry()[Name] = - [](const std::shared_ptr& exec) - { - return std::make_shared(exec); - }; - return true; - }(); - }; -} \ No newline at end of file diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 5be3fe1..69d5e32 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -35,23 +35,23 @@ void Modelec::DownAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 1.76; + msg.items[0].start_angle = action_executor_->servo_value_[0]; msg.items[0].end_angle = 2.91; msg.items[0].duration_s = 2; msg.items[1].id = 1; - msg.items[1].start_angle = 2.06; + msg.items[1].start_angle = action_executor_->servo_value_[1]; msg.items[1].end_angle = 0.95; msg.items[1].duration_s = 2; msg.items[2].id = 2; - msg.items[2].start_angle = 0.5; - msg.items[2].end_angle = 3.2; + msg.items[2].start_angle = action_executor_->servo_value_[2]; + msg.items[2].end_angle = inverted_ ? 0 : 3.2; msg.items[2].duration_s = 2; msg.items[3].id = 3; - msg.items[3].start_angle = 2.6; - msg.items[3].end_angle = 0; + msg.items[3].start_angle = action_executor_->servo_value_[3]; + msg.items[3].end_angle = inverted_ ? 3.1 : 0; msg.items[3].duration_s = 2; } @@ -97,6 +97,7 @@ void Modelec::DownAction::Init(const std::vector& params) if (!params.empty()) { SetFront(static_cast(std::stoi(params[1]))); + SetInverted(params.size() >= 3 ? (params[2] == "1" || params[2] == "true") : false); } } @@ -104,3 +105,8 @@ void Modelec::DownAction::SetFront(Front front) { front_ = front; } + +void Modelec::DownAction::SetInverted(bool inverted) +{ + inverted_ = inverted; +} diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 8bfdce9..9dcae8d 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -45,7 +45,7 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = 3; + msg.items[i].start_angle = action_executor_->servo_value_[msg.items[i].id]; msg.items[i].end_angle = 1; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/rotate_arm_action.cpp b/src/modelec_strat/src/action/rotate_arm_action.cpp deleted file mode 100644 index e261941..0000000 --- a/src/modelec_strat/src/action/rotate_arm_action.cpp +++ /dev/null @@ -1,92 +0,0 @@ -#include - -#include "modelec_strat/action_executor.hpp" - -Modelec::RotateArmAction::RotateArmAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) -{ - steps_.push(ActionExec::ROTATE_ARM_STEP); - steps_.push(ActionExec::DONE_STEP); -} - -Modelec::RotateArmAction::RotateArmAction(const std::shared_ptr& action_executor, Front front, bool rotated) - : RotateArmAction(action_executor) -{ - front_ = front; - rotated_ = rotated; -} - -void Modelec::RotateArmAction::Next() -{ - if (steps_.empty()) - { - done_ = true; - return; - } - - auto step = steps_.front(); - steps_.pop(); - - switch (step) - { - case ActionExec::ROTATE_ARM_STEP: - { - ActionServoTimedArray msg; - msg.items.resize(2); - - rotated_ = !rotated_; - - if (rotated_) - { - msg.items[0].id = 2 + front_ == FRONT ? 0 : 8; - msg.items[0].start_angle = front_ == BaseAction::FRONT ? 0.2 : 0; - msg.items[0].end_angle = front_ == BaseAction::FRONT ? 0 : 0; - msg.items[0].duration_s = 1; - - msg.items[1].id = 3 + front_ == FRONT ? 0 : 8; - msg.items[1].start_angle = front_ == BaseAction::FRONT ? 2.8 : 0; - msg.items[1].end_angle = front_ == BaseAction::FRONT ? 0 : 0; - msg.items[1].duration_s = 1; - } else - { - msg.items[0].id = 2 + front_ == FRONT ? 0 : 8; - msg.items[0].start_angle = front_ == BaseAction::FRONT ? 0 : 0; - msg.items[0].end_angle = front_ == BaseAction::FRONT ? 0.2 : 0; - msg.items[0].duration_s = 1; - - msg.items[1].id = 3 + front_ == FRONT ? 0 : 8; - msg.items[1].start_angle = front_ == BaseAction::FRONT ? 0 : 0; - msg.items[1].end_angle = front_ == BaseAction::FRONT ? 2.8 : 0; - msg.items[1].duration_s = 1; - } - - action_executor_->MoveServoTimed(msg); - } - break; - case ActionExec::DONE_STEP: - { - done_ = true; - } - break; - default: - break; - } -} - -void Modelec::RotateArmAction::Init(const std::vector& params) -{ - if (!params.empty()) - { - SetFront(static_cast(std::stoi(params[1]))); - SetRotated(params.size() >= 3 ? (params[2] == "1" || params[2] == "true") : false); - } -} - -void Modelec::RotateArmAction::SetFront(Front front) -{ - front_ = front; -} - -void Modelec::RotateArmAction::SetRotated(bool rotated) -{ - rotated_ = rotated; -} \ No newline at end of file diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 9c66ac1..cee53b5 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -45,7 +45,7 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = 1; + msg.items[i].start_angle = action_executor_->servo_value_[msg.items[i].id]; msg.items[i].end_angle = 3; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index e831452..debc9b0 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -34,22 +34,22 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 2.91; + msg.items[0].start_angle = action_executor_->servo_value_[0]; msg.items[0].end_angle = 1.76; msg.items[0].duration_s = 2; msg.items[1].id = 1; - msg.items[1].start_angle = 0.95; + msg.items[1].start_angle = action_executor_->servo_value_[1]; msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 2; msg.items[2].id = 2; - msg.items[2].start_angle = 3.2; + msg.items[2].start_angle = action_executor_->servo_value_[2]; msg.items[2].end_angle = 0.5; msg.items[2].duration_s = 2; msg.items[3].id = 3; - msg.items[3].start_angle = 0; + msg.items[3].start_angle = action_executor_->servo_value_[3]; msg.items[3].end_angle = 2.6; msg.items[3].duration_s = 2; } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 5587c84..64b40fe 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -359,7 +359,13 @@ namespace Modelec static_cast(force), static_cast(rotated)); - auto action = std::make_shared(shared_from_this(), front, rotated); + if (arm_pos_[front].down) + { + auto action = std::make_shared(shared_from_this(), front); + action_.push(action); + } + + auto action = std::make_shared(shared_from_this(), front, rotated); action_.push(action); if (action_done_) { From 55ff8ab3e65c25cf88e70b164471331a7ffff978 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:31:05 +0100 Subject: [PATCH 33/81] maybe ?? --- src/modelec_strat/src/action_executor.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 64b40fe..b3006d0 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -5,7 +5,6 @@ #include "modelec_strat/action/free_action.hpp" #include "modelec_strat/action/take_action.hpp" #include "modelec_strat/action/toggle_servo_action.hpp" -#include "modelec_strat/action/rotate_arm_action.hpp" #include "modelec_utils/utils.hpp" namespace Modelec From 79c609d32f53a620552fa93590ddd8c1abfbeb57 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:40:08 +0100 Subject: [PATCH 34/81] log --- src/modelec_strat/src/action/up_action.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index debc9b0..818b636 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -33,6 +33,19 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { + // log base servo value and next + RCLCPP_INFO( + action_executor_->GetNode()->get_logger(), + "UPAction: FRONT servo values: 0=%.2f, 1=%.2f, 2=%.2f, 3=%.2f", + action_executor_->servo_value_[0], + action_executor_->servo_value_[1], + action_executor_->servo_value_[2], + action_executor_->servo_value_[3]); + + RCLCPP_INFO( + action_executor_->GetNode()->get_logger(), + "UPAction: Moving FRONT servos to positions: 0=1.76, 1=2.06, 2=0.5, 3=2.6"); + msg.items[0].id = 0; msg.items[0].start_angle = action_executor_->servo_value_[0]; msg.items[0].end_angle = 1.76; From 9c8d6cf0059d257b16d38fa3e925c510a48396fe Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:42:16 +0100 Subject: [PATCH 35/81] default value --- .../include/modelec_strat/action_executor.hpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 9b00b8f..7ff2134 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -60,7 +60,16 @@ namespace Modelec }; std::array arm_pos_; - std::array servo_value_; + std::array servo_value_ = { + 2.91, + 0.95, + 3.2, + 0, + 1, + 1, + 1, + 1, + }; bool IsEmpty() const; From e9bf69251e7e2bc080510eef59a20b900697e184 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:49:44 +0100 Subject: [PATCH 36/81] abort mission --- src/modelec_strat/src/action/down_action.cpp | 8 ++++---- src/modelec_strat/src/action/free_action.cpp | 2 +- src/modelec_strat/src/action/take_action.cpp | 2 +- src/modelec_strat/src/action/up_action.cpp | 21 ++++---------------- 4 files changed, 10 insertions(+), 23 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 69d5e32..1193e31 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -35,22 +35,22 @@ void Modelec::DownAction::Next() if (front_ == FRONT || front_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = action_executor_->servo_value_[0]; + msg.items[0].start_angle = 1.76; msg.items[0].end_angle = 2.91; msg.items[0].duration_s = 2; msg.items[1].id = 1; - msg.items[1].start_angle = action_executor_->servo_value_[1]; + msg.items[1].start_angle = 2.06; msg.items[1].end_angle = 0.95; msg.items[1].duration_s = 2; msg.items[2].id = 2; - msg.items[2].start_angle = action_executor_->servo_value_[2]; + msg.items[2].start_angle = 0.5; msg.items[2].end_angle = inverted_ ? 0 : 3.2; msg.items[2].duration_s = 2; msg.items[3].id = 3; - msg.items[3].start_angle = action_executor_->servo_value_[3]; + msg.items[3].start_angle = 2.6; msg.items[3].end_angle = inverted_ ? 3.1 : 0; msg.items[3].duration_s = 2; } diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 9dcae8d..8bfdce9 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -45,7 +45,7 @@ void Modelec::FreeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = action_executor_->servo_value_[msg.items[i].id]; + msg.items[i].start_angle = 3; msg.items[i].end_angle = 1; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index cee53b5..8c8dd89 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -45,7 +45,7 @@ void Modelec::TakeAction::Next() for (size_t i = 0; i < servos_.size(); i++) { msg.items[i].id = servos_[i].first + (servos_[i].second ? 4 : 12); - msg.items[i].start_angle = action_executor_->servo_value_[msg.items[i].id]; + msg.items[i].start_angle = 0; msg.items[i].end_angle = 3; msg.items[i].duration_s = 0.5; } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 818b636..2bf18b9 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -33,36 +33,23 @@ void Modelec::UPAction::Next() if (front_ == FRONT || front_ == BOTH) { - // log base servo value and next - RCLCPP_INFO( - action_executor_->GetNode()->get_logger(), - "UPAction: FRONT servo values: 0=%.2f, 1=%.2f, 2=%.2f, 3=%.2f", - action_executor_->servo_value_[0], - action_executor_->servo_value_[1], - action_executor_->servo_value_[2], - action_executor_->servo_value_[3]); - - RCLCPP_INFO( - action_executor_->GetNode()->get_logger(), - "UPAction: Moving FRONT servos to positions: 0=1.76, 1=2.06, 2=0.5, 3=2.6"); - msg.items[0].id = 0; - msg.items[0].start_angle = action_executor_->servo_value_[0]; + msg.items[0].start_angle = 2.91; msg.items[0].end_angle = 1.76; msg.items[0].duration_s = 2; msg.items[1].id = 1; - msg.items[1].start_angle = action_executor_->servo_value_[1]; + msg.items[1].start_angle = 0.95; msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 2; msg.items[2].id = 2; - msg.items[2].start_angle = action_executor_->servo_value_[2]; + msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2; msg.items[2].end_angle = 0.5; msg.items[2].duration_s = 2; msg.items[3].id = 3; - msg.items[3].start_angle = action_executor_->servo_value_[3]; + msg.items[3].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 3.1 : 0; msg.items[3].end_angle = 2.6; msg.items[3].duration_s = 2; } From 67c8bec23ba570eb72f9550d5282c5255f62b8f8 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 14:54:55 +0100 Subject: [PATCH 37/81] data --- .../include/modelec_strat/action_executor.hpp | 2 +- src/modelec_strat/src/action_executor.cpp | 16 +++++++++++++--- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 7ff2134..67070ae 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -31,7 +31,7 @@ namespace Modelec void ReInit(); - void Down(BaseAction::Front front, bool force = false); + void Down(BaseAction::Front front, bool force = false, bool inverted = false); void Up(BaseAction::Front front, bool force = false); diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index b3006d0..a82b50a 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -260,7 +260,7 @@ namespace Modelec std::swap(action_, empty); } - void ActionExecutor::Down(BaseAction::Front front, bool force) { + void ActionExecutor::Down(BaseAction::Front front, bool force, bool inverted) { if (!arm_pos_[front].down || force) { RCLCPP_DEBUG( @@ -269,7 +269,7 @@ namespace Modelec static_cast(front), static_cast(force)); - auto action = std::make_shared(shared_from_this(), front); + auto action = std::make_shared(shared_from_this(), front, inverted); action_.push(action); if (action_done_) { @@ -281,10 +281,15 @@ namespace Modelec { arm_pos_[BaseAction::FRONT].down = true; arm_pos_[BaseAction::BACK].down = true; + + arm_pos_[BaseAction::FRONT].rotated = inverted; + arm_pos_[BaseAction::BACK].rotated = inverted; } else { arm_pos_[front].down = true; + + arm_pos_[front].rotated = inverted; } Update(); @@ -343,7 +348,7 @@ namespace Modelec } else { - Down(front, force); + Down(front, force, arm_pos_[front].rotated); } } @@ -376,10 +381,15 @@ namespace Modelec { arm_pos_[BaseAction::FRONT].rotated = rotated; arm_pos_[BaseAction::BACK].rotated = rotated; + + arm_pos_[BaseAction::FRONT].down = true; + arm_pos_[BaseAction::BACK].down = true; } else { arm_pos_[front].rotated = rotated; + + arm_pos_[front].down = true; } Update(); From f64f02ba6cb2df5d90e3ac5c023275c08094c9a8 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 30 Jan 2026 21:02:01 +0100 Subject: [PATCH 38/81] start thermo mission --- build.rasp.sh | 2 +- build.sh | 2 +- build.zsh | 2 +- compilation_database.build.zsh | 3 + src/modelec_com/src/pcb_odo_interface.cpp | 9 +- src/modelec_strat/CMakeLists.txt | 1 + src/modelec_strat/data/config.xml | 7 +- .../modelec_strat/missions/thermo_mission.hpp | 40 +++++ .../modelec_strat/navigation_helper.hpp | 19 +- .../include/modelec_strat/strat_fms.hpp | 3 + src/modelec_strat/src/action/down_action.cpp | 12 +- src/modelec_strat/src/action/up_action.cpp | 16 +- .../src/missions/thermo_mission.cpp | 169 ++++++++++++++++++ src/modelec_strat/src/navigation_helper.cpp | 7 +- src/modelec_strat/src/pathfinding.cpp | 2 +- src/modelec_strat/src/strat_fms.cpp | 6 +- 16 files changed, 260 insertions(+), 40 deletions(-) create mode 100755 compilation_database.build.zsh create mode 100644 src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp create mode 100644 src/modelec_strat/src/missions/thermo_mission.cpp diff --git a/build.rasp.sh b/build.rasp.sh index 517e550..92cfa84 100755 --- a/build.rasp.sh +++ b/build.rasp.sh @@ -1,4 +1,4 @@ #!/bin/bash -MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON # -DCMAKE_BUILD_TYPE=Debug +MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential # --cmake-args -DCMAKE_BUILD_TYPE=Debug source install/setup.bash diff --git a/build.sh b/build.sh index 947ccff..54fc2eb 100755 --- a/build.sh +++ b/build.sh @@ -1,4 +1,4 @@ #!/bin/bash -colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +colcon build --symlink-install --cmake-args # -DCMAKE_EXPORT_COMPILE_COMMANDS=ON source install/setup.bash diff --git a/build.zsh b/build.zsh index fd96718..bfb17da 100755 --- a/build.zsh +++ b/build.zsh @@ -1,3 +1,3 @@ -colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug +colcon build # --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug source "install/setup.zsh" diff --git a/compilation_database.build.zsh b/compilation_database.build.zsh new file mode 100755 index 0000000..c340a78 --- /dev/null +++ b/compilation_database.build.zsh @@ -0,0 +1,3 @@ +colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON # -DCMAKE_BUILD_TYPE=Debug + +source "install/setup.zsh" diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index cf6b8b7..11a2a96 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -102,11 +102,8 @@ namespace Modelec "odometry/start", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) { - if (msg->data != start_odo_) - { - start_odo_ = msg->data; - SendOrder("START", {std::to_string(msg->data)}); - } + start_odo_ = msg->data; + SendOrder("START", {std::to_string(msg->data)}); }); this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); @@ -179,7 +176,7 @@ namespace Modelec { if (tokens[2] == "REACH") { - RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); + RCLCPP_DEBUG(this->get_logger(), "Waypoint reached: ID %s", tokens[3].c_str()); int id = std::stoi(tokens[3]); diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index bd2cdfb..a92c07d 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -30,6 +30,7 @@ set(strat_fsm_sources src/missions/go_home_mission.cpp src/missions/take_mission.cpp src/missions/free_mission.cpp + src/missions/thermo_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 41ac8a2..61cf027 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -71,5 +71,10 @@ 77 65 - true + false + + 20 + 0.6 + + 20 \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp new file mode 100644 index 0000000..2fb713d --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include +#include + +namespace Modelec { + class ThermoMission : public Mission { + public: + ThermoMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front = BaseAction::FRONT); + + void Start(rclcpp::Node::SharedPtr node) override; + void Update() override; + void Clear() override; + MissionStatus GetStatus() const override; + std::string GetName() const override { return "Thermo"; } + + private: + enum Step + { + }; + + BaseAction::Front front_; + + std::shared_ptr closestBox; + MissionStatus status_; + std::shared_ptr nav_; + std::shared_ptr action_executor_; + rclcpp::Node::SharedPtr node_; + + rclcpp::Time go_timeout_; + rclcpp::Time deploy_time_; + + std::optional min_time_; + + rclcpp::Time last_ask_waypoint_time_; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index e7e0101..52950cc 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -140,6 +140,7 @@ namespace Modelec Point spawn_; float factor_close_enemy_ = 0; + float factor_theta_ = 0; int enemy_emergency_distance_ = 0; @@ -184,7 +185,7 @@ namespace Modelec std::shared_ptr closest_obstacle = nullptr; auto robotPos = Point(pos->x, pos->y, pos->theta); auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta); - float distance = std::numeric_limits::max(); + double score = std::numeric_limits::max(); for (const auto& obstacle : GetPathfinding()->GetObstacles()) { @@ -192,17 +193,15 @@ namespace Modelec { if (!obs->IsAtObjective()) { - auto dist = Point::distance(robotPos, obs->GetPosition()); + auto obsPoint = obs->GetPosition(); + double distance = Point::distance(robotPos, obsPoint); + double enemy_distance = Point::distance(enemyPos, obsPoint); + double theta = std::abs(Point::angleDiff(robotPos, obsPoint)); - if (has_enemy_) + double s = distance + (enemy_distance * factor_close_enemy_ * has_enemy_) + theta * factor_theta_; + if (s < score) { - auto enemyDist = Point::distance(enemyPos, obs->GetPosition()); - dist *= (1.0f + factor_close_enemy_ * std::exp(-enemyDist / enemy_emergency_distance_)); - } - - if (dist < distance) - { - distance = dist; + score = s; closest_obstacle = obs; } } diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 29fb244..92ada0d 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -64,6 +64,9 @@ namespace Modelec std::queue game_action_sequence_; bool static_strat_ = false; + float factor_obs_; + int timer_period_ms_ = 100; + std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 1193e31..793f502 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -37,22 +37,22 @@ void Modelec::DownAction::Next() msg.items[0].id = 0; msg.items[0].start_angle = 1.76; msg.items[0].end_angle = 2.91; - msg.items[0].duration_s = 2; + msg.items[0].duration_s = 0.5; msg.items[1].id = 1; msg.items[1].start_angle = 2.06; msg.items[1].end_angle = 0.95; - msg.items[1].duration_s = 2; + msg.items[1].duration_s = 0.5; msg.items[2].id = 2; msg.items[2].start_angle = 0.5; msg.items[2].end_angle = inverted_ ? 0 : 3.2; - msg.items[2].duration_s = 2; + msg.items[2].duration_s = 0.5; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; msg.items[3].end_angle = inverted_ ? 3.1 : 0; - msg.items[3].duration_s = 2; + msg.items[3].duration_s = 0.5; } if (front_ == BACK || front_ == BOTH) @@ -61,12 +61,12 @@ void Modelec::DownAction::Next() msg.items[i].id = 8; msg.items[i].start_angle = 0; msg.items[i].end_angle = 0; - msg.items[i].duration_s = 1; + msg.items[i].duration_s = 0.5; msg.items[i+1].id = 9; msg.items[i+1].start_angle = 0; msg.items[i+1].end_angle = 0; - msg.items[i+1].duration_s = 1; + msg.items[i+1].duration_s = 0.5; msg.items[i+2].id = 10; msg.items[i+2].start_angle = 0; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 2bf18b9..8c8e596 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -36,22 +36,22 @@ void Modelec::UPAction::Next() msg.items[0].id = 0; msg.items[0].start_angle = 2.91; msg.items[0].end_angle = 1.76; - msg.items[0].duration_s = 2; + msg.items[0].duration_s = 0.5; msg.items[1].id = 1; msg.items[1].start_angle = 0.95; msg.items[1].end_angle = 2.06; - msg.items[1].duration_s = 2; + msg.items[1].duration_s = 0.5; msg.items[2].id = 2; msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2; msg.items[2].end_angle = 0.5; - msg.items[2].duration_s = 2; + msg.items[2].duration_s = 0.5; msg.items[3].id = 3; msg.items[3].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 3.1 : 0; msg.items[3].end_angle = 2.6; - msg.items[3].duration_s = 2; + msg.items[3].duration_s = 0.5; } if (front_ == BACK || front_ == BOTH) { @@ -60,22 +60,22 @@ void Modelec::UPAction::Next() msg.items[i].id = 8; msg.items[i].start_angle = 0; msg.items[i].end_angle = 0; - msg.items[i].duration_s = 1; + msg.items[i].duration_s = 0.5; msg.items[i+1].id = 9; msg.items[i+1].start_angle = 0; msg.items[i+1].end_angle = 0; - msg.items[i+1].duration_s = 1; + msg.items[i+1].duration_s = 0.5; msg.items[i+2].id = 10; msg.items[i+2].start_angle = 0; msg.items[i+2].end_angle = 0; - msg.items[i+2].duration_s = 1; + msg.items[i+2].duration_s = 0.5; msg.items[i+3].id = 11; msg.items[i+3].start_angle = 0; msg.items[i+3].end_angle = 0; - msg.items[i+3].duration_s = 1; + msg.items[i+3].duration_s = 0.5; } action_executor_->MoveServoTimed(msg); diff --git a/src/modelec_strat/src/missions/thermo_mission.cpp b/src/modelec_strat/src/missions/thermo_mission.cpp new file mode 100644 index 0000000..59ea345 --- /dev/null +++ b/src/modelec_strat/src/missions/thermo_mission.cpp @@ -0,0 +1,169 @@ +#include + +#include "modelec_strat/action/base_action.hpp" + +namespace Modelec { + ThermoMission::ThermoMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor, + BaseAction::Front front) + : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + { + } + + void ThermoMission::Start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + go_timeout_ = node_->now(); + deploy_time_ = node_->now(); + last_ask_waypoint_time_ = node_->now(); + + status_ = MissionStatus::RUNNING; + + std::queue empty; + std::swap(steps_, empty); + + /*steps_.push(GO_TO_TAKE); + steps_.push(GO_TO_TAKE_CLOSE); + steps_.push(DOWN); + steps_.push(TAKE); + steps_.push(UP); + steps_.push(DONE);*/ + } + + void ThermoMission::Update() + { + if (!action_executor_->IsActionDone()) + { + return; + } + + if (!nav_->HasArrived()) + { + if ((node_->now() - go_timeout_).seconds() < 2 && (node_->now() - last_ask_waypoint_time_).seconds() > 1) + { + nav_->AskWaypoint(); + last_ask_waypoint_time_ = node_->now(); + return; + } + if ((node_->now() - go_timeout_).seconds() < 10) + { + return; + } + } + + if (min_time_.has_value()) + { + if ((node_->now() - min_time_.value()).seconds() < 0.1) + { + return; + } + else + { + min_time_.reset(); + } + } + + auto step_ = steps_.front(); + steps_.pop(); + + /*switch (step_) + { + case GO_TO_TAKE: + { + + closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); + + if (closestBox == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + action_executor_->box_obstacles_[front_] = closestBox; + + auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); + + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } + } + } + + go_timeout_ = node_->now(); + } + break; + case GO_TO_TAKE_CLOSE: + { + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } + + go_timeout_ = node_->now(); + } + break; + case DOWN: + { + action_executor_->Down(front_); + deploy_time_ = node_->now(); + } + break; + case TAKE: + { + action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); + deploy_time_ = node_->now(); + min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); + } + break; + case UP: + { + action_executor_->Up(front_); + deploy_time_ = node_->now(); + } + break; + case DONE: + { + if (action_executor_->box_obstacles_[front_] == nullptr) + { + status_ = MissionStatus::FAILED; + break; + } + + nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + } + + status_ = MissionStatus::DONE; + break; + default: + break; + }*/ + } + + void ThermoMission::Clear() + { + } + + MissionStatus ThermoMission::GetStatus() const + { + return status_; + } + +} diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 2607f02..d79cf29 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -13,6 +13,7 @@ namespace Modelec pathfinding_ = std::make_shared(node); factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); + factor_theta_ = Config::get("config.factor.theta", 20.f); enemy_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 390); @@ -518,9 +519,9 @@ namespace Modelec { auto zonePoint = zone->GetPosition(); double distance = Point::distance(posPoint, zonePoint); - double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); + double enemy_distance = Point::distance(enemyPos, zonePoint); double theta = std::abs(Point::angleDiff(posPoint, zonePoint)); - double s = distance + enemy_distance * factor_close_enemy_ + theta * 2; + double s = distance + (enemy_distance * factor_close_enemy_ * has_enemy_) + theta * factor_theta_; if (s < score) { score = s; @@ -722,7 +723,7 @@ namespace Modelec void NavigationHelper::AskWaypoint() { - RCLCPP_INFO(node_->get_logger(), "Asking for active waypoint..."); + RCLCPP_DEBUG(node_->get_logger(), "Asking for active waypoint..."); std_msgs::msg::Empty msg; odo_ask_waypoint_pub_->publish(msg); } diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index 923ab72..c6d4bf2 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -262,7 +262,7 @@ namespace Modelec { const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y); // log the x and y in the real format - RCLCPP_INFO(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x * (int) cell_size_mm_x, + RCLCPP_DEBUG(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x * (int) cell_size_mm_x, (grid_height_ - 1 - start_y) * (int) cell_size_mm_y, goal_x * (int) cell_size_mm_x, (grid_height_ - 1 - goal_y) * (int) cell_size_mm_y); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 8ddedbb..9333b76 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -67,6 +67,8 @@ namespace Modelec } static_strat_ = Config::get("config.static_strat", false); + factor_obs_ = Config::get("config.factor.obs", 1.0); + timer_period_ms_ = Config::get("config.timer_period_ms", 100); } void StratFMS::Init() @@ -104,7 +106,7 @@ namespace Modelec current_mission_.reset(); match_start_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME); - timer_ = create_wall_timer(std::chrono::milliseconds(100), [this] + timer_ = create_wall_timer(std::chrono::milliseconds(timer_period_ms_), [this] { Update(); }); @@ -233,7 +235,7 @@ namespace Modelec if (closestBox && closestDeposite) { double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), - closestBox->GetPosition()); + closestBox->GetPosition()) * factor_obs_; double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), closestDeposite->GetPosition()); From ddfda3909bbb0d6b81a58ff8d0c89bac632982ee Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 31 Jan 2026 11:15:57 +0100 Subject: [PATCH 39/81] rewrite action --- .../modelec_strat/action/base_action.hpp | 2 + .../modelec_strat/action/down_action.hpp | 2 + .../modelec_strat/action/free_action.hpp | 2 + .../modelec_strat/action/take_action.hpp | 2 + .../action/toggle_servo_action.hpp | 2 + .../modelec_strat/action/up_action.hpp | 2 + .../include/modelec_strat/action_executor.hpp | 2 + src/modelec_strat/src/action/down_action.cpp | 18 ++++++ src/modelec_strat/src/action/free_action.cpp | 9 +++ src/modelec_strat/src/action/take_action.cpp | 9 +++ .../src/action/toggle_servo_action.cpp | 9 +++ src/modelec_strat/src/action/up_action.cpp | 13 ++++ src/modelec_strat/src/action_executor.cpp | 64 +++---------------- 13 files changed, 80 insertions(+), 56 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/action/base_action.hpp b/src/modelec_strat/include/modelec_strat/action/base_action.hpp index cb4b103..3e3f253 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -55,6 +55,8 @@ namespace Modelec const std::string& action_name, const std::shared_ptr& action_executor); + virtual void End() = 0; + protected: std::shared_ptr action_executor_; diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index 4a64957..84ced7c 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -16,6 +16,8 @@ namespace Modelec void SetFront(Front front); void SetInverted(bool inverted); + void End() override; + inline static const std::string Name = ActionExec::DOWN; private: diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index 3a4532d..87a4d35 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -19,6 +19,8 @@ namespace Modelec void AddServo(std::pair servo); void AddServos(const std::vector>& servos); + void End() override; + inline static const std::string Name = ActionExec::FREE; private: diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index 68cd7a6..f7e5f9d 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -19,6 +19,8 @@ namespace Modelec void AddServo(std::pair servo); void AddServos(const std::vector>& servos); + void End() override; + inline static const std::string Name = ActionExec::TAKE; private: diff --git a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp index d27c438..49c3eb4 100644 --- a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp @@ -19,6 +19,8 @@ namespace Modelec void AddServo(std::pair servo); void AddServos(const std::vector>& servos); + void End() override; + inline static const std::string Name = ActionExec::TOGGLE_SERVO; private: diff --git a/src/modelec_strat/include/modelec_strat/action/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 95b8ea8..5d43611 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -15,6 +15,8 @@ namespace Modelec void Init(const std::vector& params) override; void SetFront(Front front); + void End() override; + inline static const std::string Name = ActionExec::UP; private: diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 67070ae..60966ba 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -49,6 +49,8 @@ namespace Modelec void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); + void ActionFinished(const std::shared_ptr& action); + std::array, 2> box_obstacles_; std::array servo_pos_; diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 793f502..14a9da4 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -110,3 +110,21 @@ void Modelec::DownAction::SetInverted(bool inverted) { inverted_ = inverted; } + +void Modelec::DownAction::End() +{ + if (front_ == BOTH) + { + action_executor_->arm_pos_[FRONT].down = true; + action_executor_->arm_pos_[BACK].down = true; + + action_executor_->arm_pos_[FRONT].rotated = inverted_; + action_executor_->arm_pos_[BACK].rotated = inverted_; + } + else + { + action_executor_->arm_pos_[front_].down = true; + + action_executor_->arm_pos_[front_].rotated = inverted_; + } +} diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 8bfdce9..188b00e 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -90,3 +90,12 @@ void Modelec::FreeAction::AddServos(const std::vector>& se { servos_.insert(servos_.end(), servos.begin(), servos.end()); } + +void Modelec::FreeAction::End() +{ + for (auto servo : servos_) + { + auto index = servo.first + (servo.second == FRONT ? 0 : 4); + action_executor_->servo_pos_[index] = false; + } +} diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 8c8dd89..0e85bfe 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -90,3 +90,12 @@ void Modelec::TakeAction::AddServos(const std::vector>& se { servos_.insert(servos_.end(), servos.begin(), servos.end()); } + +void Modelec::TakeAction::End() +{ + for (auto servo : servos_) + { + auto index = servo.first + (servo.second == FRONT ? 0 : 4); + action_executor_->servo_pos_[index] = true; + } +} diff --git a/src/modelec_strat/src/action/toggle_servo_action.cpp b/src/modelec_strat/src/action/toggle_servo_action.cpp index 5ed9f8a..391372b 100644 --- a/src/modelec_strat/src/action/toggle_servo_action.cpp +++ b/src/modelec_strat/src/action/toggle_servo_action.cpp @@ -90,3 +90,12 @@ void Modelec::ToggleServoAction::AddServos(const std::vectorservo_pos_[index] = !action_executor_->servo_pos_[index]; + } +} diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 8c8e596..dec71dd 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -103,3 +103,16 @@ void Modelec::UPAction::SetFront(Front front) { front_ = front; } + +void Modelec::UPAction::End() +{ + if (front_ == BOTH) + { + action_executor_->arm_pos_[FRONT].down = false; + action_executor_->arm_pos_[BACK].down = false; + } + else + { + action_executor_->arm_pos_[front_].down = false; + } +} diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index a82b50a..69dc429 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -225,6 +225,7 @@ namespace Modelec action->Next(); if (action->IsDone()) { + ActionFinished(action); action_.pop(); if (action_.empty()) { @@ -241,6 +242,8 @@ namespace Modelec node_->get_logger(), "ActionExecutor Update: Action is done, step_running_=%d", step_running_); + ActionFinished(action); + action_.pop(); if (action_.empty()) { @@ -277,21 +280,6 @@ namespace Modelec } action_done_ = false; - if (front == BaseAction::BOTH) - { - arm_pos_[BaseAction::FRONT].down = true; - arm_pos_[BaseAction::BACK].down = true; - - arm_pos_[BaseAction::FRONT].rotated = inverted; - arm_pos_[BaseAction::BACK].rotated = inverted; - } - else - { - arm_pos_[front].down = true; - - arm_pos_[front].rotated = inverted; - } - Update(); } } @@ -313,16 +301,6 @@ namespace Modelec } action_done_ = false; - if (front == BaseAction::BOTH) - { - arm_pos_[BaseAction::FRONT].down = false; - arm_pos_[BaseAction::BACK].down = false; - } - else - { - arm_pos_[front].down = false; - } - Update(); } } @@ -377,21 +355,6 @@ namespace Modelec } action_done_ = false; - if (front == BaseAction::BOTH) - { - arm_pos_[BaseAction::FRONT].rotated = rotated; - arm_pos_[BaseAction::BACK].rotated = rotated; - - arm_pos_[BaseAction::FRONT].down = true; - arm_pos_[BaseAction::BACK].down = true; - } - else - { - arm_pos_[front].rotated = rotated; - - arm_pos_[front].down = true; - } - Update(); } } @@ -410,12 +373,6 @@ namespace Modelec } action_done_ = false; - - for (auto servo : servos) - { - servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = true; - } - Update(); } @@ -433,11 +390,6 @@ namespace Modelec } action_done_ = false; - for (auto servo : servos) - { - servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = false; - } - Update(); } @@ -456,11 +408,6 @@ namespace Modelec } action_done_ = false; - for (auto servo : servos) - { - servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)] = !servo_pos_[servo.first + (servo.second == BaseAction::FRONT ? 0 : 4)]; - } - Update(); } @@ -482,6 +429,11 @@ namespace Modelec step_running_ += msg.items.size(); } + void ActionExecutor::ActionFinished(const std::shared_ptr& action) + { + action->End(); + } + bool ActionExecutor::IsEmpty() const { return box_obstacles_[0] == nullptr && box_obstacles_[1] == nullptr; From a974e9c713ed02d639e4a86688b86d2e5e5db888 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 31 Jan 2026 13:10:30 +0100 Subject: [PATCH 40/81] ThermoMission --- .../msg/Strat/StratState.msg | 1 + src/modelec_strat/data/config.xml | 11 ++ src/modelec_strat/data/obstacles.xml | 4 +- .../include/modelec_strat/action_executor.hpp | 10 ++ .../modelec_strat/missions/thermo_mission.hpp | 13 +- .../modelec_strat/navigation_helper.hpp | 1 + .../include/modelec_strat/strat_fms.hpp | 2 + src/modelec_strat/src/action_executor.cpp | 19 +++ .../src/missions/free_mission.cpp | 10 +- .../src/missions/thermo_mission.cpp | 92 ++++++-------- src/modelec_strat/src/navigation_helper.cpp | 27 ++++ src/modelec_strat/src/strat_fms.cpp | 115 ++++++++++++------ 12 files changed, 193 insertions(+), 112 deletions(-) diff --git a/src/modelec_interfaces/msg/Strat/StratState.msg b/src/modelec_interfaces/msg/Strat/StratState.msg index 89c45a0..0d4c8ae 100644 --- a/src/modelec_interfaces/msg/Strat/StratState.msg +++ b/src/modelec_interfaces/msg/Strat/StratState.msg @@ -5,6 +5,7 @@ int32 SELECT_GAME_ACTION=3 int32 TAKE_MISSION=10 int32 FREE_MISSION=11 +int32 THERMO_MISSION=12 int32 DO_GO_HOME=20 int32 STOP=21 diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 61cf027..2fec31e 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -75,6 +75,17 @@ 20 0.6 + 0.7 20 + + + + + + + + + + \ No newline at end of file diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index 151de31..d4ccf6d 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -1,8 +1,6 @@ - - - + diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 60966ba..f290349 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -8,8 +8,10 @@ #include #include +#include #include "action/base_action.hpp" +#include "missions/thermo_mission.hpp" #include "obstacle/box.hpp" namespace Modelec @@ -49,6 +51,10 @@ namespace Modelec void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); + void ActivateThermo(int teamId); + + void DeactivateThermo(int teamId); + void ActionFinished(const std::shared_ptr& action); std::array, 2> box_obstacles_; @@ -85,6 +91,8 @@ namespace Modelec bool HasOneBox() const; + void SendPoint(int point) const; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; @@ -99,6 +107,8 @@ namespace Modelec rclcpp::Subscription::SharedPtr action_exec_sub_; rclcpp::Subscription::SharedPtr joy_sub_; + rclcpp::Publisher::SharedPtr score_pub_; + std::queue> action_; bool action_done_ = true; diff --git a/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp index 2fb713d..b2d90ea 100644 --- a/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/thermo_mission.hpp @@ -8,8 +8,7 @@ namespace Modelec { class ThermoMission : public Mission { public: ThermoMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor, - BaseAction::Front front = BaseAction::FRONT); + const std::shared_ptr& action_executor); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -17,12 +16,20 @@ namespace Modelec { MissionStatus GetStatus() const override; std::string GetName() const override { return "Thermo"; } + static bool IsThermoDone; + private: enum Step { + GO_TO_THERMO, + GO_TO_THERMO_CLOSE, + ACTIVATE_THERMO, + GO_TO_10, + DEACTIVATE_THERMO, + DONE, }; - BaseAction::Front front_; + std::array thermo_positions_; std::shared_ptr closestBox; MissionStatus status_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 52950cc..84a87c1 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -95,6 +95,7 @@ namespace Modelec std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; PosMsg::SharedPtr GetHomePosition(); + std::array GetThermoPositions(); void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 92ada0d..a7938a6 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -28,6 +28,7 @@ namespace Modelec TAKE_MISSION = modelec_interfaces::msg::StratState::TAKE_MISSION, FREE_MISSION = modelec_interfaces::msg::StratState::FREE_MISSION, + THERMO_MISSION = modelec_interfaces::msg::StratState::THERMO_MISSION, DO_GO_HOME = modelec_interfaces::msg::StratState::DO_GO_HOME, STOP = modelec_interfaces::msg::StratState::STOP, @@ -65,6 +66,7 @@ namespace Modelec bool static_strat_ = false; float factor_obs_; + float factor_thermo_; int timer_period_ms_ = 100; std::shared_ptr nav_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 69dc429..974d65d 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -201,6 +201,8 @@ namespace Modelec } } }); + + score_pub_ = node_->create_publisher("/strat/score", 10); } rclcpp::Node::SharedPtr ActionExecutor::GetNode() const @@ -429,6 +431,16 @@ namespace Modelec step_running_ += msg.items.size(); } + void ActionExecutor::ActivateThermo(int) + { + // TODO : do something + } + + void ActionExecutor::DeactivateThermo(int) + { + // TODO : do something + } + void ActionExecutor::ActionFinished(const std::shared_ptr& action) { action->End(); @@ -463,4 +475,11 @@ namespace Modelec { return HasFrontBox() != HasBackBox(); } + + void ActionExecutor::SendPoint(const int point) const + { + std_msgs::msg::Int64 msg; + msg.data = point; + score_pub_->publish(msg); + } } diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 10aa9b2..4ee42ae 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -82,17 +82,9 @@ namespace Modelec { return; } - auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta), - nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0); - auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); - auto pos = depoPoint.GetTakePosition(dist); - - RCLCPP_INFO( - node_->get_logger(), - "FreeMission: position (%.2d, %.2d) with distance %.2f", - pos.x, pos.y, dist); + auto pos = depoPoint.GetTakePosition(200.0); pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; diff --git a/src/modelec_strat/src/missions/thermo_mission.cpp b/src/modelec_strat/src/missions/thermo_mission.cpp index 59ea345..cb9715c 100644 --- a/src/modelec_strat/src/missions/thermo_mission.cpp +++ b/src/modelec_strat/src/missions/thermo_mission.cpp @@ -3,10 +3,12 @@ #include "modelec_strat/action/base_action.hpp" namespace Modelec { + + bool ThermoMission::IsThermoDone = false; + ThermoMission::ThermoMission(const std::shared_ptr& nav, - const std::shared_ptr& action_executor, - BaseAction::Front front) - : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + const std::shared_ptr& action_executor) + : status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -20,15 +22,17 @@ namespace Modelec { status_ = MissionStatus::RUNNING; + thermo_positions_ = nav_->GetThermoPositions(); + std::queue empty; std::swap(steps_, empty); - /*steps_.push(GO_TO_TAKE); - steps_.push(GO_TO_TAKE_CLOSE); - steps_.push(DOWN); - steps_.push(TAKE); - steps_.push(UP); - steps_.push(DONE);*/ + steps_.push(GO_TO_THERMO); + steps_.push(GO_TO_THERMO_CLOSE); + steps_.push(ACTIVATE_THERMO); + steps_.push(GO_TO_10); + steps_.push(DEACTIVATE_THERMO); + steps_.push(DONE); } void ThermoMission::Update() @@ -67,51 +71,26 @@ namespace Modelec { auto step_ = steps_.front(); steps_.pop(); - /*switch (step_) + switch (step_) { - case GO_TO_TAKE: + case GO_TO_THERMO: { + auto start = thermo_positions_[0].GetTakePosition(CLOSE_DISTANCE, M_PI_2); - closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); - - if (closestBox == nullptr) + if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; } - action_executor_->box_obstacles_[front_] = closestBox; - - auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); - - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) - { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) - { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) - { - status_ = MissionStatus::FAILED; - break; - } - } - } - go_timeout_ = node_->now(); } break; - case GO_TO_TAKE_CLOSE: + case GO_TO_THERMO_CLOSE: { - if (action_executor_->box_obstacles_[front_] == nullptr) - { - status_ = MissionStatus::FAILED; - break; - } + auto start = thermo_positions_[0]; - auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; - - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(start, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; @@ -120,41 +99,40 @@ namespace Modelec { go_timeout_ = node_->now(); } break; - case DOWN: + case ACTIVATE_THERMO: { - action_executor_->Down(front_); + action_executor_->ActivateThermo(nav_->GetTeamId()); deploy_time_ = node_->now(); } break; - case TAKE: + case GO_TO_10: { - action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); - deploy_time_ = node_->now(); - min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); + auto pos = thermo_positions_[1]; + + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + break; + } } break; - case UP: + case DEACTIVATE_THERMO: { - action_executor_->Up(front_); + action_executor_->DeactivateThermo(nav_->GetTeamId()); deploy_time_ = node_->now(); } break; case DONE: { - if (action_executor_->box_obstacles_[front_] == nullptr) - { - status_ = MissionStatus::FAILED; - break; - } - - nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + action_executor_->SendPoint(10); + IsThermoDone = true; } status_ = MissionStatus::DONE; break; default: break; - }*/ + } } void ThermoMission::Clear() diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index d79cf29..70f6240 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -551,6 +551,33 @@ namespace Modelec return home; } + std::array NavigationHelper::GetThermoPositions() + { + Point thermoPosStart = Point(); + Point thermoPosFinish = Point(); + if (team_id_ == YELLOW) + { + thermoPosStart.x = Config::get("config.thermo.yellow.start@x", 0); + thermoPosStart.y = Config::get("config.thermo.yellow.start@y", 0); + thermoPosStart.theta = Config::get("config.thermo.yellow.start@theta", 0); + + thermoPosFinish.x = Config::get("config.thermo.yellow.finish@x", 0); + thermoPosFinish.y = Config::get("config.thermo.yellow.finish@y", 0); + thermoPosFinish.theta = Config::get("config.thermo.yellow.finish@theta", 0); + } + else + { + thermoPosStart.x = Config::get("config.thermo.blue.start@x", 0); + thermoPosStart.y = Config::get("config.thermo.blue.start@y", 0); + thermoPosStart.theta = Config::get("config.thermo.blue.start@theta", 0); + + thermoPosFinish.x = Config::get("config.thermo.blue.finish@x", 0); + thermoPosFinish.y = Config::get("config.thermo.blue.finish@y", 0); + thermoPosFinish.theta = Config::get("config.thermo.blue.finish@theta", 0); + } + return {thermoPosStart, thermoPosFinish}; + } + void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { if (!has_enemy_) has_enemy_ = true; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 9333b76..44412c8 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -6,6 +6,7 @@ #include #include #include +#include #include "modelec_strat/action/base_action.hpp" @@ -68,6 +69,7 @@ namespace Modelec static_strat_ = Config::get("config.static_strat", false); factor_obs_ = Config::get("config.factor.obs", 1.0); + factor_thermo_ = Config::get("config.factor.thermo", 0.5); timer_period_ms_ = Config::get("config.timer_period_ms", 100); } @@ -215,57 +217,69 @@ namespace Modelec // TODO : If close to border, do the side mission (thermometre) + auto pos = nav_->GetCurrentPos(); + auto closestBox = nav_->GetClosestObstacle(pos); + auto closestDeposite = nav_->GetClosestDepositeZone(pos); - if (action_executor_->IsFull()) - { - RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - else if (action_executor_->IsEmpty()) + auto thermoPos = nav_->GetThermoPositions()[0]; + + double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), + closestBox->GetPosition()) * factor_obs_; + double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), + closestDeposite->GetPosition()); + double distToThermo = Point::distance(Point(pos->x, pos->y, pos->theta), + thermoPos) * factor_thermo_; + + if (distToThermo < distToBox && distToThermo < distToDeposite && !ThermoMission::IsThermoDone) { - RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else + RCLCPP_INFO(get_logger(), "Choosing THERMO mission (dist to thermo: %.2f < dist to box: %.2f, dist to deposite: %.2f)", + distToThermo, distToBox, distToDeposite); + Transition(State::THERMO_MISSION, "Selecting THERMO mission"); + } else { - auto pos = nav_->GetCurrentPos(); - auto closestBox = nav_->GetClosestObstacle(pos); - auto closestDeposite = nav_->GetClosestDepositeZone(pos); - - if (closestBox && closestDeposite) + if (action_executor_->IsFull()) + { + RCLCPP_INFO(get_logger(), "All box are present on robot, selecting FREE mission"); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + else if (action_executor_->IsEmpty()) + { + RCLCPP_INFO(get_logger(), "No box present on robot, selecting TAKE mission"); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else { - double distToBox = Point::distance(Point(pos->x, pos->y, pos->theta), - closestBox->GetPosition()) * factor_obs_; - double distToDeposite = Point::distance(Point(pos->x, pos->y, pos->theta), - closestDeposite->GetPosition()); - if (distToBox < distToDeposite) + if (closestBox && closestDeposite) + { + if (distToBox < distToDeposite) + { + RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", + distToBox, distToDeposite); + Transition(State::TAKE_MISSION, "Selecting TAKE mission"); + } + else + { + RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", + distToDeposite, distToBox); + Transition(State::FREE_MISSION, "Selecting FREE mission"); + } + } + else if (closestBox) { - RCLCPP_INFO(get_logger(), "Choosing TAKE mission (dist to box: %.2f < dist to deposite: %.2f)", - distToBox, distToDeposite); + RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); Transition(State::TAKE_MISSION, "Selecting TAKE mission"); } - else + else if (closestDeposite) { - RCLCPP_INFO(get_logger(), "Choosing FREE mission (dist to deposite: %.2f < dist to box: %.2f)", - distToDeposite, distToBox); + RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); Transition(State::FREE_MISSION, "Selecting FREE mission"); } - } - else if (closestBox) - { - RCLCPP_INFO(get_logger(), "Only box available, selecting TAKE mission"); - Transition(State::TAKE_MISSION, "Selecting TAKE mission"); - } - else if (closestDeposite) - { - RCLCPP_INFO(get_logger(), "Only deposite available, selecting FREE mission"); - Transition(State::FREE_MISSION, "Selecting FREE mission"); - } - else - { - RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); - Transition(State::STOP, "No missions available"); + else + { + RCLCPP_WARN(get_logger(), "No box or deposite available, cannot select mission!"); + Transition(State::STOP, "No missions available"); + } } } } @@ -344,6 +358,27 @@ namespace Modelec } break; + case State::THERMO_MISSION: + { + if (!current_mission_) + { + current_mission_ = std::make_unique(nav_, action_executor_); + current_mission_->Start(shared_from_this()); + } + current_mission_->Update(); + if (current_mission_->GetStatus() == MissionStatus::DONE) + { + current_mission_.reset(); + Transition(State::SELECT_MISSION, "Thermo done"); + } + else if (current_mission_->GetStatus() == MissionStatus::FAILED) + { + current_mission_.reset(); + RCLCPP_ERROR(get_logger(), "Thermo mission failed!"); + Transition(State::SELECT_MISSION, "Thermo mission failed"); + } + } + break; case State::DO_GO_HOME: if (!current_mission_) { From 3d824e8576de79c32c24b74a2c9e84e7b81a6823 Mon Sep 17 00:00:00 2001 From: acki Date: Sun, 1 Feb 2026 13:56:47 +0100 Subject: [PATCH 41/81] reduce a lot cpu consumption of map page --- .../include/modelec_gui/pages/map_page.hpp | 23 ++- src/modelec_gui/src/pages/map_page.cpp | 170 +++++++++++++++++- src/modelec_strat/data/config.xml | 2 +- src/modelec_strat/src/strat_fms.cpp | 3 + 4 files changed, 190 insertions(+), 8 deletions(-) diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index ddc4ce0..79fac1d 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include @@ -40,15 +41,19 @@ namespace ModelecGUI { protected: void paintEvent(QPaintEvent*) override; - void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg); void resizeEvent(QResizeEvent* event) override; + void updateBackgroundCache(); + + void updateObstaclesCache(); + + void updateWaypointsCache(); + rclcpp::TimerBase::SharedPtr reset_timer_; - QSvgRenderer* renderer; + QSvgRenderer* renderer_; QVBoxLayout* v_layout; QHBoxLayout* h_layout; @@ -97,5 +102,17 @@ namespace ModelecGUI { long int start_time_ = 0; rclcpp::Subscription::SharedPtr strat_state_sub_; + + QPixmap background_cache_; + QPixmap obstacles_cache_; + QPixmap waypoints_cache_; + + bool bg_dirty_ = true; + bool obstacles_dirty_ = true; + bool waypoints_dirty_ = true; + + QPixmap robot_texture_; + QPixmap top_texture_; + QPixmap obs_texture_; }; } diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 9a2fe1f..fce114f 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -9,9 +9,13 @@ namespace ModelecGUI { MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), - renderer(new QSvgRenderer( + renderer_(new QSvgRenderer( QString(":/img/playmat/2026_FINAL.svg"), - this)), node_(node) + this)), + node_(node), + robot_texture_(":/img/logo/modelec.png"), + top_texture_(":/img/logo/ISEN-Nantes.png"), + obs_texture_(":/img/wood.jpg") { ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; @@ -71,6 +75,8 @@ namespace ModelecGUI qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_)); + + waypoints_dirty_ = true; update(); }); @@ -90,6 +96,7 @@ namespace ModelecGUI height() - point.y * ratioBetweenMapAndWidgetY_)); } + waypoints_dirty_ = true; update(); }); @@ -113,6 +120,8 @@ namespace ModelecGUI [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { OnObstacleReceived(msg); + obstacles_dirty_ = true; + update(); }); obstacle_removed_sub_ = node_->create_subscription( @@ -120,6 +129,8 @@ namespace ModelecGUI [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.erase(msg->id); + obstacles_dirty_ = true; + update(); }); enemy_pos_sub_ = node_->create_subscription("enemy/position", 10, @@ -185,6 +196,14 @@ namespace ModelecGUI auto result2 = ask_map_obstacle_client_->async_send_request(std::make_shared()); rclcpp::spin_until_future_complete(node_->get_node_base_interface(), result2); + + auto timer = new QTimer(this); + connect(timer, &QTimer::timeout, this, [this]() { + if (!isGameStarted_) return; + auto elapsed = (std::chrono::system_clock::now().time_since_epoch().count() - start_time_) / 1e9; + timer_label_->setText(QString("%1 s").arg(elapsed)); + }); + timer->start(1000); } void MapPage::AskMap() @@ -228,7 +247,44 @@ namespace ModelecGUI { QWidget::paintEvent(paint_event); - if (isGameStarted_) + QPainter painter(this); + + if (bg_dirty_) updateBackgroundCache(); + painter.drawPixmap(0, 0, background_cache_); + + if (show_obstacle_) + { + if (obstacles_dirty_) updateObstaclesCache(); + painter.drawPixmap(0, 0, obstacles_cache_); + } + + if (waypoints_dirty_) updateWaypointsCache(); + painter.drawPixmap(0, 0, waypoints_cache_); + + // --- robot --- + painter.save(); + painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_); + painter.rotate(90 - robotPos.theta * 180.0 / M_PI); + + QRect r(-robot_width_/2, -robot_length_/2, + robot_width_, robot_length_); + + painter.drawPixmap(r, robot_texture_); + painter.restore(); + + // --- Enemy --- + if (hasEnemy) + { + painter.setBrush(Qt::red); + painter.drawRect( + (enemy_pos_.x - enemy_width_/2) * ratioBetweenMapAndWidgetX_, + height() - (enemy_pos_.y + enemy_length_/2) * ratioBetweenMapAndWidgetY_, + enemy_width_ * ratioBetweenMapAndWidgetX_, + enemy_length_ * ratioBetweenMapAndWidgetY_); + } + + /*if (isGameStarted_) { auto now = std::chrono::system_clock::now().time_since_epoch(); auto start = std::chrono::nanoseconds(start_time_); @@ -352,12 +408,14 @@ namespace ModelecGUI painter.setPen(QPen(Qt::black, 5)); painter.drawRect(rect); - painter.drawPixmap(imageRect.topLeft(), texture); + painter.drawPixmap(imageRect.topLeft(), texture);*/ } void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_[msg->id] = *msg; + obstacles_dirty_ = true; + update(); } void MapPage::resizeEvent(QResizeEvent* event) @@ -366,5 +424,109 @@ namespace ModelecGUI ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; + + bg_dirty_ = true; + obstacles_dirty_ = true; + waypoints_dirty_ = true; + update(); + } + + void MapPage::updateBackgroundCache() + { + background_cache_ = QPixmap(size()); + background_cache_.fill(Qt::transparent); + + QPainter p(&background_cache_); + renderer_->render(&p, rect()); + bg_dirty_ = false; + } + + void MapPage::updateObstaclesCache() + { + obstacles_cache_ = QPixmap(size()); + obstacles_cache_.fill(Qt::transparent); + + QPainter painter(&obstacles_cache_); + painter.setRenderHint(QPainter::Antialiasing); + + for (auto& [index, obs] : obstacle_) + { + painter.save(); + + QPoint pos(obs.x * ratioBetweenMapAndWidgetX_, + height() - obs.y * ratioBetweenMapAndWidgetY_); + painter.translate(pos); + painter.rotate(90 - obs.theta * 180.0 / M_PI); + + if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN) + { + painter.setBrush(QBrush(obs_texture_)); + } + else if (obs.id == 2) + { + + auto texture = top_texture_.scaled(obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio); + + QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); + + QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + painter.drawRect(toDraw); + + painter.drawPixmap(imageRect.topLeft(), texture); + + painter.restore(); + + continue; + } + else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE) + { + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + } + else + { + painter.setBrush(Qt::red); + painter.setOpacity(0.5); + painter.setPen(QPen(Qt::red, 5)); + } + + QRect r(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_); + + painter.drawRect(r); + + painter.restore(); + } + + obstacles_dirty_ = false; + } + + void MapPage::updateWaypointsCache() + { + waypoints_cache_ = QPixmap(size()); + waypoints_cache_.fill(Qt::transparent); + + QPainter painter(&waypoints_cache_); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(QPen(Qt::red, 2)); + + for (size_t i = 0; i + 1 < qpoints.size(); ++i) + painter.drawLine(qpoints[i], qpoints[i + 1]); + + painter.setPen(Qt::NoPen); + painter.setBrush(Qt::red); + + for (auto& p : qpoints) + painter.drawEllipse(p, 5, 5); + + waypoints_dirty_ = false; } } diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 2fec31e..fd3c30c 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -71,7 +71,7 @@ 77 65 - false + true 20 0.6 diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 44412c8..e967095 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -153,7 +153,10 @@ namespace Modelec auto empty_queue_ = std::queue(); std::swap(game_action_sequence_, empty_queue_); game_action_sequence_.push(State::TAKE_MISSION); + game_action_sequence_.push(State::TAKE_MISSION); + game_action_sequence_.push(State::FREE_MISSION); game_action_sequence_.push(State::FREE_MISSION); + game_action_sequence_.push(State::THERMO_MISSION); Transition(State::WAIT_START, "System ready"); } From 038fc9d6d5f8fd67848c4750b94dbf93e875cce6 Mon Sep 17 00:00:00 2001 From: acki Date: Sun, 1 Feb 2026 14:16:28 +0100 Subject: [PATCH 42/81] reduce a lot cpu consumption --- src/modelec_gui/src/pages/map_page.cpp | 126 ------------------------ src/modelec_strat/src/enemy_manager.cpp | 2 +- src/modelec_strat/src/strat_fms.cpp | 2 - 3 files changed, 1 insertion(+), 129 deletions(-) diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index fce114f..7ae002f 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -283,132 +283,6 @@ namespace ModelecGUI enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); } - - /*if (isGameStarted_) - { - auto now = std::chrono::system_clock::now().time_since_epoch(); - auto start = std::chrono::nanoseconds(start_time_); - auto elapsed = now - start; - auto elapsed_s = std::chrono::duration_cast(elapsed).count(); - timer_label_->setText(QString::number(elapsed_s) + " s"); - } - - QPainter painter(this); - renderer->render(&painter, rect()); // Scales SVG to widget size - painter.save(); - - painter.setRenderHint(QPainter::Antialiasing); - - // --- Draw lines --- - painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - for (size_t i = 0; i + 1 < qpoints.size(); ++i) - { - painter.drawLine(qpoints[i], qpoints[i + 1]); - } - - painter.setPen(Qt::NoPen); - - // --- Draw colored points --- - const int radius = 5; - - for (size_t i = 0; i < qpoints.size(); ++i) - { - if (i == qpoints.size() - 1) - painter.setBrush(Qt::blue); // Last = blue - else - painter.setBrush(Qt::red); // Middle = red - - painter.drawEllipse(qpoints[i], radius, radius); - } - - painter.restore(); - - if (show_obstacle_) - { - for (auto& [index, obs] : obstacle_) - { - painter.save(); - - QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_); - painter.translate(obsPoint); - painter.rotate(90 - obs.theta * (180.0 / M_PI)); - - if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN) - { - QPixmap texture(":/img/wood.jpg"); - painter.setBrush(QBrush(texture)); - } - else if (obs.id == 2) - { - QPixmap texture(":/img/logo/ISEN-Nantes.png"); - texture = texture.scaled(obs.width * ratioBetweenMapAndWidgetX_, - obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio); - - QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); - - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), - -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); - - painter.setBrush(Qt::white); - painter.setPen(Qt::NoPen); - painter.drawRect(toDraw); - - painter.drawPixmap(imageRect.topLeft(), texture); - - painter.restore(); - - continue; - } - else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE) - { - painter.setBrush(Qt::white); - painter.setPen(Qt::NoPen); - } - else - { - painter.setBrush(Qt::red); - painter.setOpacity(0.5); - painter.setPen(QPen(Qt::red, 5)); - } - - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), - -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); - - painter.drawRect(toDraw); - - painter.restore(); - } - - // -- Draw enemy position -- - if (hasEnemy) - { - painter.setBrush(Qt::red); - painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, - height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, - enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); - } - } - - // -- Draw robot position -- - painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_); - painter.rotate(90 - robotPos.theta * (180.0 / M_PI)); - - QPixmap texture(":/img/logo/modelec.png"); - texture = texture.scaled(robot_width_ * ratioBetweenMapAndWidgetX_ * 0.9, - robot_length_ * ratioBetweenMapAndWidgetY_ * 0.9, Qt::KeepAspectRatio); - - QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); - - QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2), - robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_); - - painter.setBrush(Qt::white); - painter.setPen(QPen(Qt::black, 5)); - painter.drawRect(rect); - - painter.drawPixmap(imageRect.topLeft(), texture);*/ } void MapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 47b80d3..750149d 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -80,7 +80,7 @@ namespace Modelec }); timer_ = this->create_wall_timer( - std::chrono::nanoseconds(static_cast(refresh_rate_s_ )), + std::chrono::seconds(static_cast(refresh_rate_s_)), [this]() { TimerCallback(); diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index e967095..298d0ba 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -218,8 +218,6 @@ namespace Modelec return; } - - // TODO : If close to border, do the side mission (thermometre) auto pos = nav_->GetCurrentPos(); auto closestBox = nav_->GetClosestObstacle(pos); auto closestDeposite = nav_->GetClosestDepositeZone(pos); From 5dd5f56a3cf1b4324c105b20ef9c97bd0de3b91e Mon Sep 17 00:00:00 2001 From: acki Date: Sun, 1 Feb 2026 15:50:14 +0100 Subject: [PATCH 43/81] size --- src/modelec_gui/src/pages/map_page.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 7ae002f..2d0c1ee 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -267,8 +267,8 @@ namespace ModelecGUI height() - robotPos.y * ratioBetweenMapAndWidgetY_); painter.rotate(90 - robotPos.theta * 180.0 / M_PI); - QRect r(-robot_width_/2, -robot_length_/2, - robot_width_, robot_length_); + QRect r(-(robot_width_*ratioBetweenMapAndWidgetX_)/2, -(robot_length_*ratioBetweenMapAndWidgetY_)/2, + (robot_width_*ratioBetweenMapAndWidgetX_), (robot_length_*ratioBetweenMapAndWidgetY_)); painter.drawPixmap(r, robot_texture_); painter.restore(); From bd6e22fc4f370a4bd1b4ac397facd70873bfbf8c Mon Sep 17 00:00:00 2001 From: acki Date: Sun, 1 Feb 2026 23:05:32 +0100 Subject: [PATCH 44/81] beginning color setup --- .../joy.ros2_launch_marcel.desktop | 0 .../no_lidar.joy.ros2_launch_marcel.desktop | 0 .../no_lidar.ros2_launch_marcel.desktop | 0 .../ros2_launch_marcel.desktop | 0 TODO.md | 4 - install.sh | 6 +- src/modelec_gui/src/pages/action_page.cpp | 4 +- .../msg/Action/ActionExec.msg | 8 +- src/modelec_strat/CMakeLists.txt | 2 + .../modelec_strat/action/base_action.hpp | 5 +- .../modelec_strat/action/down_action.hpp | 6 +- .../modelec_strat/action/free_action.hpp | 14 +-- .../modelec_strat/action/look_on_action.hpp | 38 ++++++++ .../modelec_strat/action/take_action.hpp | 14 +-- .../modelec_strat/action/thermo_action.hpp | 40 +++++++++ .../action/toggle_servo_action.hpp | 14 +-- .../modelec_strat/action/up_action.hpp | 6 +- .../include/modelec_strat/action_executor.hpp | 29 ++++--- .../modelec_strat/missions/free_mission.hpp | 8 +- .../modelec_strat/missions/take_mission.hpp | 4 +- .../modelec_strat/navigation_helper.hpp | 8 +- .../include/modelec_strat/obstacle/box.hpp | 19 ++++ src/modelec_strat/src/action/down_action.cpp | 26 +++--- src/modelec_strat/src/action/free_action.cpp | 20 ++--- .../src/action/look_on_action.cpp | 69 +++++++++++++++ src/modelec_strat/src/action/take_action.cpp | 20 ++--- .../src/action/thermo_action.cpp | 87 +++++++++++++++++++ .../src/action/toggle_servo_action.cpp | 20 ++--- src/modelec_strat/src/action/up_action.cpp | 22 ++--- src/modelec_strat/src/action_executor.cpp | 86 +++++++++++------- .../src/missions/free_mission.cpp | 69 +++++++++++---- .../src/missions/take_mission.cpp | 40 +++++---- .../src/missions/thermo_mission.cpp | 6 +- src/modelec_strat/src/navigation_helper.cpp | 4 +- src/modelec_strat/src/obstacle/box.cpp | 35 ++++++++ src/modelec_strat/src/strat_fms.cpp | 7 +- 36 files changed, 556 insertions(+), 184 deletions(-) rename joy.ros2_launch_marcel.desktop => Desktop/joy.ros2_launch_marcel.desktop (100%) rename no_lidar.joy.ros2_launch_marcel.desktop => Desktop/no_lidar.joy.ros2_launch_marcel.desktop (100%) rename no_lidar.ros2_launch_marcel.desktop => Desktop/no_lidar.ros2_launch_marcel.desktop (100%) rename ros2_launch_marcel.desktop => Desktop/ros2_launch_marcel.desktop (100%) delete mode 100644 TODO.md create mode 100644 src/modelec_strat/include/modelec_strat/action/look_on_action.hpp create mode 100644 src/modelec_strat/include/modelec_strat/action/thermo_action.hpp create mode 100644 src/modelec_strat/src/action/look_on_action.cpp create mode 100644 src/modelec_strat/src/action/thermo_action.cpp diff --git a/joy.ros2_launch_marcel.desktop b/Desktop/joy.ros2_launch_marcel.desktop similarity index 100% rename from joy.ros2_launch_marcel.desktop rename to Desktop/joy.ros2_launch_marcel.desktop diff --git a/no_lidar.joy.ros2_launch_marcel.desktop b/Desktop/no_lidar.joy.ros2_launch_marcel.desktop similarity index 100% rename from no_lidar.joy.ros2_launch_marcel.desktop rename to Desktop/no_lidar.joy.ros2_launch_marcel.desktop diff --git a/no_lidar.ros2_launch_marcel.desktop b/Desktop/no_lidar.ros2_launch_marcel.desktop similarity index 100% rename from no_lidar.ros2_launch_marcel.desktop rename to Desktop/no_lidar.ros2_launch_marcel.desktop diff --git a/ros2_launch_marcel.desktop b/Desktop/ros2_launch_marcel.desktop similarity index 100% rename from ros2_launch_marcel.desktop rename to Desktop/ros2_launch_marcel.desktop diff --git a/TODO.md b/TODO.md deleted file mode 100644 index a5d8687..0000000 --- a/TODO.md +++ /dev/null @@ -1,4 +0,0 @@ -- API (check for the middleware already implemented or fill the API package) - - -- Check every value and after that update the max TIME mission \ No newline at end of file diff --git a/install.sh b/install.sh index 3e7cbef..75a8c44 100755 --- a/install.sh +++ b/install.sh @@ -27,9 +27,9 @@ git submodule init git submodule update echo "source /opt/ros/jazzy/setup.bash -source ~/Modelec-ROS/install/setup.bash +source ~/Modelec-ROS2/install/setup.bash export RMW_IMPLEMENTATION=rmw_fastrtps_cpp -export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml +export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS2/fastdds_setup.xml export ROS_DOMAIN_ID=128" >> ~/.bashrc source ~/.bashrc @@ -39,7 +39,7 @@ source src/rplidar_ros/scripts/create_udev_rules.sh cd ../.. -cp ./*.desktop ~/Desktop +cp ./Desktop/*.desktop ~/Desktop chmod +x ~/Desktop/*.desktop gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" true gio set ~/Desktop/no_lidar.ros2_launch_marcel.desktop "metadata::trusted" true diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index b420e2c..0c48742 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -238,7 +238,7 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::THERMO_DEPLOY; + action_exec.action = ActionExec::THERMO + ActionExec::DELIMITER + "-1" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -246,7 +246,7 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::THERMO_UNDEPLOY; + action_exec.action = ActionExec::THERMO + ActionExec::DELIMITER + "-1" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); diff --git a/src/modelec_interfaces/msg/Action/ActionExec.msg b/src/modelec_interfaces/msg/Action/ActionExec.msg index a7841fd..c9f91f2 100644 --- a/src/modelec_interfaces/msg/Action/ActionExec.msg +++ b/src/modelec_interfaces/msg/Action/ActionExec.msg @@ -8,8 +8,8 @@ string ROTATE_ARM = "ROTATE_ARM" string TAKE = "TAKE" string FREE = "FREE" string TOGGLE_SERVO = "TOGGLE_SERVO" -string THERMO_DEPLOY = "THERMO_DEPLOY" -string THERMO_UNDEPLOY = "THERMO_UNDEPLOY" +string THERMO = "THERMO" +string LOOK_ON = "LOOK_ON" string MAX_DEPLOY = "MAX_DEPLOY" # Step @@ -18,11 +18,11 @@ int32 UP_STEP = 1 int32 DOWN_STEP = 2 int32 TAKE_STEP = 3 int32 FREE_STEP = 4 -int32 THERMO_DEPLOY_STEP = 5 -int32 THERMO_UNDEPLOY_STEP = 6 +int32 THERMO_STEP = 5 int32 TOGGLE_SERVO_STEP = 7 int32 TOGGLE_ARM_STEP = 8 int32 ROTATE_ARM_STEP = 9 +int32 LOOK_ON_STEP = 10 int32 MAX_DEPLOY_STEP = 99 int32[] steps diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index a92c07d..7db4dee 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -26,6 +26,8 @@ set(strat_fsm_sources src/action/free_action.cpp src/action/take_action.cpp src/action/toggle_servo_action.cpp + src/action/look_on_action.cpp + src/action/thermo_action.cpp src/missions/go_home_mission.cpp src/missions/take_mission.cpp diff --git a/src/modelec_strat/include/modelec_strat/action/base_action.hpp b/src/modelec_strat/include/modelec_strat/action/base_action.hpp index 3e3f253..944f34a 100644 --- a/src/modelec_strat/include/modelec_strat/action/base_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/base_action.hpp @@ -25,11 +25,14 @@ namespace Modelec class BaseAction { public: - enum Front + enum Side { FRONT = 1, BACK = 0, BOTH = -1, + LEFT = 2, + RIGHT = 3, + CENTER = 4, }; using Ptr = std::shared_ptr; diff --git a/src/modelec_strat/include/modelec_strat/action/down_action.hpp b/src/modelec_strat/include/modelec_strat/action/down_action.hpp index 84ced7c..0884b7a 100644 --- a/src/modelec_strat/include/modelec_strat/action/down_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/down_action.hpp @@ -9,11 +9,11 @@ namespace Modelec { public: DownAction(const std::shared_ptr& action_executor); - DownAction(const std::shared_ptr& action_executor, Front front, bool inverted = false); + DownAction(const std::shared_ptr& action_executor, Side side, bool inverted = false); void Next() override; void Init(const std::vector& params) override; - void SetFront(Front front); + void SetSide(Side side); void SetInverted(bool inverted); void End() override; @@ -21,7 +21,7 @@ namespace Modelec inline static const std::string Name = ActionExec::DOWN; private: - Front front_; + Side side_; bool inverted_; diff --git a/src/modelec_strat/include/modelec_strat/action/free_action.hpp b/src/modelec_strat/include/modelec_strat/action/free_action.hpp index 87a4d35..fcfb79e 100644 --- a/src/modelec_strat/include/modelec_strat/action/free_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/free_action.hpp @@ -9,22 +9,22 @@ namespace Modelec { public: FreeAction(const std::shared_ptr& action_executor); - FreeAction(const std::shared_ptr& action_executor, Front front, int n); - FreeAction(const std::shared_ptr& action_executor, std::pair servo); - FreeAction(const std::shared_ptr& action_executor, std::vector> servos); + FreeAction(const std::shared_ptr& action_executor, Side side, int n); + FreeAction(const std::shared_ptr& action_executor, std::pair servo); + FreeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, Front front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Side side); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); void End() override; inline static const std::string Name = ActionExec::FREE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp b/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp new file mode 100644 index 0000000..0534cbf --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/look_on_action.hpp @@ -0,0 +1,38 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + class LookOnAction : public BaseAction + { + public: + LookOnAction(const std::shared_ptr& action_executor); + LookOnAction(const std::shared_ptr& action_executor, Side side); + + void Next() override; + void Init(const std::vector& params) override; + void SetSide(Side side); + + void End() override; + + inline static const std::string Name = ActionExec::LOOK_ON; + + private: + Side side_ = CENTER; + + std::queue steps_; + + inline static const bool registered_ = + []() + { + BaseAction::Registry()[Name] = + [](const std::shared_ptr& exec) + { + return std::make_shared(exec); + }; + return true; + }(); + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/action/take_action.hpp b/src/modelec_strat/include/modelec_strat/action/take_action.hpp index f7e5f9d..0ad3de6 100644 --- a/src/modelec_strat/include/modelec_strat/action/take_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/take_action.hpp @@ -9,22 +9,22 @@ namespace Modelec { public: TakeAction(const std::shared_ptr& action_executor); - TakeAction(const std::shared_ptr& action_executor, Front front, int n); - TakeAction(const std::shared_ptr& action_executor, std::pair servo); - TakeAction(const std::shared_ptr& action_executor, std::vector> servos); + TakeAction(const std::shared_ptr& action_executor, Side side, int n); + TakeAction(const std::shared_ptr& action_executor, std::pair servo); + TakeAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, Front front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Side side); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); void End() override; inline static const std::string Name = ActionExec::TAKE; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp b/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp new file mode 100644 index 0000000..9cf1ac6 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/action/thermo_action.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include +#include + +namespace Modelec +{ + class ThermoAction : public BaseAction + { + public: + ThermoAction(const std::shared_ptr& action_executor); + ThermoAction(const std::shared_ptr& action_executor, Side side, bool deploy); + + void Next() override; + void Init(const std::vector& params) override; + void SetSide(Side side); + void SetDeploy(bool deploy); + + void End() override; + + inline static const std::string Name = ActionExec::THERMO; + + private: + Side side_ = BOTH; + bool deploy_ = true; + + std::queue steps_; + + inline static const bool registered_ = + []() + { + BaseAction::Registry()[Name] = + [](const std::shared_ptr& exec) + { + return std::make_shared(exec); + }; + return true; + }(); + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp index 49c3eb4..76c8484 100644 --- a/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/toggle_servo_action.hpp @@ -9,22 +9,22 @@ namespace Modelec { public: ToggleServoAction(const std::shared_ptr& action_executor); - ToggleServoAction(const std::shared_ptr& action_executor, Front front, int n); - ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo); - ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos); + ToggleServoAction(const std::shared_ptr& action_executor, Side side, int n); + ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo); + ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos); void Next() override; void Init(const std::vector& params) override; - void AddServo(int id, Front front); - void AddServo(std::pair servo); - void AddServos(const std::vector>& servos); + void AddServo(int id, Side side); + void AddServo(std::pair servo); + void AddServos(const std::vector>& servos); void End() override; inline static const std::string Name = ActionExec::TOGGLE_SERVO; private: - std::vector> servos_; + std::vector> servos_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action/up_action.hpp b/src/modelec_strat/include/modelec_strat/action/up_action.hpp index 5d43611..46b980d 100644 --- a/src/modelec_strat/include/modelec_strat/action/up_action.hpp +++ b/src/modelec_strat/include/modelec_strat/action/up_action.hpp @@ -9,18 +9,18 @@ namespace Modelec { public: UPAction(const std::shared_ptr& action_executor); - UPAction(const std::shared_ptr& action_executor, Front front); + UPAction(const std::shared_ptr& action_executor, Side side); void Next() override; void Init(const std::vector& params) override; - void SetFront(Front front); + void SetSide(Side side); void End() override; inline static const std::string Name = ActionExec::UP; private: - Front front_; + Side side_; std::queue steps_; diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index f290349..9f633ed 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -33,27 +33,27 @@ namespace Modelec void ReInit(); - void Down(BaseAction::Front front, bool force = false, bool inverted = false); + void Down(BaseAction::Side side, bool force = false, bool inverted = false); - void Up(BaseAction::Front front, bool force = false); + void Up(BaseAction::Side side, bool force = false); - void ToggleArm(BaseAction::Front front, bool force = false); + void ToggleArm(BaseAction::Side side, bool force = false); - void RotateArm(BaseAction::Front front, bool force = false, bool rotated = false); + void RotateArm(BaseAction::Side side, bool force = false, bool rotated = false); - void Take(const std::vector>& servos); + void Take(const std::vector>& servos); - void Free(const std::vector>& servos); + void Free(const std::vector>& servos); - void ToggleServo(const std::vector>& servos); + void ToggleServo(const std::vector>& servos); void MoveServoTimed(const modelec_interfaces::msg::ActionServoTimedArray& msg); void MoveServo(const modelec_interfaces::msg::ActionServoPosArray& msg); - void ActivateThermo(int teamId); + void ActivateThermo(BaseAction::Side side, bool deploy, bool force = false); - void DeactivateThermo(int teamId); + void LookOn(BaseAction::Side side, bool force = false); void ActionFinished(const std::shared_ptr& action); @@ -79,11 +79,20 @@ namespace Modelec 1, }; + std::map thermo_state_ = { + {BaseAction::LEFT, false}, + {BaseAction::RIGHT, false}, + }; + + BaseAction::Side cam_side_ = BaseAction::Side::CENTER; + + bool looking_on_front_ = true; + bool IsEmpty() const; bool IsFull() const; - bool HasBox(BaseAction::Front front) const; + bool HasBox(BaseAction::Side side) const; bool HasFrontBox() const; diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index 0bdd862..eab80ce 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -9,7 +9,7 @@ namespace Modelec { public: FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front = BaseAction::FRONT); + BaseAction::Side side = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -22,13 +22,15 @@ namespace Modelec { { GO_TO_FREE, DOWN, - FREE, + FREE_FIRST, + ROTATE_ARM, + FREE_OTHER, UP, GO_BACK, DONE, }; - BaseAction::Front front_; + BaseAction::Side side_; MissionStatus status_; std::shared_ptr nav_; diff --git a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index eba1206..494cc6d 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -9,7 +9,7 @@ namespace Modelec { public: TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front = BaseAction::FRONT); + BaseAction::Side side = BaseAction::FRONT); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; @@ -28,7 +28,7 @@ namespace Modelec { DONE, }; - BaseAction::Front front_; + BaseAction::Side side_; std::shared_ptr closestBox; MissionStatus status_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 84a87c1..8ca9a91 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -19,7 +19,7 @@ namespace Modelec class NavigationHelper { public: - enum + enum Team { YELLOW = 0, BLUE = 1, @@ -35,7 +35,7 @@ namespace Modelec std::shared_ptr GetPathfinding() const; - int GetTeamId() const; + Team GetTeamId() const; void Update(); @@ -108,7 +108,7 @@ namespace Modelec bool Replan(bool force = false); - void SetTeamId(int id); + void SetTeamId(Team id); void SetSpawn(const std::string& name); @@ -135,7 +135,7 @@ namespace Modelec std::shared_ptr pathfinding_; - int team_id_ = YELLOW; + Team team_id_ = YELLOW; std::map spawn_yellow_; std::map spawn_blue_; Point spawn_; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 18f5f8a..a72aa4a 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -8,6 +8,12 @@ namespace Modelec class BoxObstacle : public Obstacle { public: + enum Team + { + YELLOW = 0, + BLUE = 1, + }; + BoxObstacle() = default; BoxObstacle(const BoxObstacle&) = default; @@ -20,8 +26,21 @@ namespace Modelec std::vector GetAllPossiblePositions() const; + void SetColor(int index, Team team); + Team GetColor(int index) const; + std::array GetColors() const; + + std::vector GetSide(Team team) const; + protected: std::vector possible_angles_; + + std::array colors_ = { + YELLOW, + BLUE, + YELLOW, + BLUE + }; }; } diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 14a9da4..ac3db4f 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -2,15 +2,15 @@ #include "modelec_strat/action_executor.hpp" -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor), front_(BOTH), inverted_(false) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor) : BaseAction(action_executor), side_(BOTH), inverted_(false) { steps_.push(ActionExec::DOWN_STEP); steps_.push(ActionExec::DONE_STEP); } -Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Front front, bool inverted) : DownAction(action_executor) +Modelec::DownAction::DownAction(const std::shared_ptr& action_executor, Side side, bool inverted) : DownAction(action_executor) { - front_ = front; + side_ = side; inverted_ = inverted; } @@ -30,9 +30,9 @@ void Modelec::DownAction::Next() case ActionExec::DOWN_STEP: { ActionServoTimedArray msg; - msg.items.resize(front_ == BOTH ? 8 : 4); + msg.items.resize(side_ == BOTH ? 8 : 4); - if (front_ == FRONT || front_ == BOTH) + if (side_ == FRONT || side_ == BOTH) { msg.items[0].id = 0; msg.items[0].start_angle = 1.76; @@ -55,9 +55,9 @@ void Modelec::DownAction::Next() msg.items[3].duration_s = 0.5; } - if (front_ == BACK || front_ == BOTH) + if (side_ == BACK || side_ == BOTH) { - int i = front_ == BOTH ? 4 : 0; + int i = side_ == BOTH ? 4 : 0; msg.items[i].id = 8; msg.items[i].start_angle = 0; msg.items[i].end_angle = 0; @@ -96,14 +96,14 @@ void Modelec::DownAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(static_cast(std::stoi(params[1]))); + SetSide(static_cast(std::stoi(params[1]))); SetInverted(params.size() >= 3 ? (params[2] == "1" || params[2] == "true") : false); } } -void Modelec::DownAction::SetFront(Front front) +void Modelec::DownAction::SetSide(Side side) { - front_ = front; + side_ = side; } void Modelec::DownAction::SetInverted(bool inverted) @@ -113,7 +113,7 @@ void Modelec::DownAction::SetInverted(bool inverted) void Modelec::DownAction::End() { - if (front_ == BOTH) + if (side_ == BOTH) { action_executor_->arm_pos_[FRONT].down = true; action_executor_->arm_pos_[BACK].down = true; @@ -123,8 +123,8 @@ void Modelec::DownAction::End() } else { - action_executor_->arm_pos_[front_].down = true; + action_executor_->arm_pos_[side_].down = true; - action_executor_->arm_pos_[front_].rotated = inverted_; + action_executor_->arm_pos_[side_].rotated = inverted_; } } diff --git a/src/modelec_strat/src/action/free_action.cpp b/src/modelec_strat/src/action/free_action.cpp index 188b00e..30cb1a4 100644 --- a/src/modelec_strat/src/action/free_action.cpp +++ b/src/modelec_strat/src/action/free_action.cpp @@ -8,17 +8,17 @@ Modelec::FreeAction::FreeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Front front, int n) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, Side side, int n) : FreeAction(action_executor) { - AddServo(n, front); + AddServo(n, side); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::pair servo) : FreeAction(action_executor) { AddServo(servo.first, servo.second); } -Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) +Modelec::FreeAction::FreeAction(const std::shared_ptr& action_executor, std::vector> servos) : FreeAction(action_executor) { AddServos(servos); } @@ -70,23 +70,23 @@ void Modelec::FreeAction::Init(const std::vector& params) for (size_t i = 1; i < params.size(); i += 2) { int id = std::stoi(params[i]); - bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front ? FRONT : BACK); + bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true; + AddServo(id, side ? FRONT : BACK); } } } -void Modelec::FreeAction::AddServo(int id, Front front) +void Modelec::FreeAction::AddServo(int id, Side side) { - servos_.emplace_back(id, front); + servos_.emplace_back(id, side); } -void Modelec::FreeAction::AddServo(std::pair servo) +void Modelec::FreeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::FreeAction::AddServos(const std::vector>& servos) +void Modelec::FreeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/look_on_action.cpp b/src/modelec_strat/src/action/look_on_action.cpp new file mode 100644 index 0000000..5663bd9 --- /dev/null +++ b/src/modelec_strat/src/action/look_on_action.cpp @@ -0,0 +1,69 @@ +#include + +#include "modelec_strat/action_executor.hpp" + +Modelec::LookOnAction::LookOnAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +{ + steps_.push(ActionExec::LOOK_ON_STEP); + steps_.push(ActionExec::DONE_STEP); +} + +Modelec::LookOnAction::LookOnAction(const std::shared_ptr& action_executor, Side side) : LookOnAction(action_executor) +{ + side_ = side; +} + +void Modelec::LookOnAction::Next() +{ + if (steps_.empty()) + { + done_ = true; + return; + } + + auto step = steps_.front(); + steps_.pop(); + + switch (step) + { + case ActionExec::LOOK_ON_STEP: + { + modelec_interfaces::msg::ActionServoTimedArray msg; + + msg.items.resize(1); + + msg.items[0].id = 18; + msg.items[0].start_angle = action_executor_->cam_side_ == CENTER ? 1 : action_executor_->cam_side_ == FRONT ? 0 : 2; + msg.items[0].end_angle = side_ == CENTER ? 1 : side_ == FRONT ? 0 : 2; + msg.items[0].duration_s = 0.5; + + action_executor_->MoveServoTimed(msg); + } + break; + case ActionExec::DONE_STEP: + { + done_ = true; + } + break; + default: + break; + } +} + +void Modelec::LookOnAction::Init(const std::vector& params) +{ + if (!params.empty()) + { + SetSide(static_cast(std::stoi(params[0]))); + } +} + +void Modelec::LookOnAction::SetSide(Side side) +{ + side_ = side; +} + +void Modelec::LookOnAction::End() +{ + action_executor_->cam_side_ = side_; +} diff --git a/src/modelec_strat/src/action/take_action.cpp b/src/modelec_strat/src/action/take_action.cpp index 0e85bfe..91752e5 100644 --- a/src/modelec_strat/src/action/take_action.cpp +++ b/src/modelec_strat/src/action/take_action.cpp @@ -8,17 +8,17 @@ Modelec::TakeAction::TakeAction(const std::shared_ptr& action_ex steps_.push(ActionExec::DONE_STEP); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Front front, int n) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, Side side, int n) : TakeAction(action_executor) { - AddServo(n, front); + AddServo(n, side); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::pair servo) : TakeAction(action_executor) { AddServo(servo); } -Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) +Modelec::TakeAction::TakeAction(const std::shared_ptr& action_executor, std::vector> servos) : TakeAction(action_executor) { AddServos(servos); } @@ -70,23 +70,23 @@ void Modelec::TakeAction::Init(const std::vector& params) for (size_t i = 1; i < params.size(); i += 2) { int id = std::stoi(params[i]); - bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front ? FRONT : BACK); + bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true; + AddServo(id, side ? FRONT : BACK); } } } -void Modelec::TakeAction::AddServo(int id, Front front) +void Modelec::TakeAction::AddServo(int id, Side side) { - servos_.emplace_back(id, front); + servos_.emplace_back(id, side); } -void Modelec::TakeAction::AddServo(std::pair servo) +void Modelec::TakeAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::TakeAction::AddServos(const std::vector>& servos) +void Modelec::TakeAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/thermo_action.cpp b/src/modelec_strat/src/action/thermo_action.cpp new file mode 100644 index 0000000..3a395bc --- /dev/null +++ b/src/modelec_strat/src/action/thermo_action.cpp @@ -0,0 +1,87 @@ +#include + +#include "modelec_strat/action_executor.hpp" + +Modelec::ThermoAction::ThermoAction(const std::shared_ptr& action_executor) : BaseAction(action_executor) +{ + steps_.push(ActionExec::THERMO_STEP); + steps_.push(ActionExec::DONE_STEP); +} + +Modelec::ThermoAction::ThermoAction(const std::shared_ptr& action_executor, Side side, bool deploy) : ThermoAction(action_executor) +{ + side_ = side; + deploy_ = deploy; +} + +void Modelec::ThermoAction::Next() +{ + if (steps_.empty()) + { + done_ = true; + return; + } + + auto step = steps_.front(); + steps_.pop(); + + switch (step) + { + case ActionExec::THERMO_STEP: + { + modelec_interfaces::msg::ActionServoTimedArray msg; + + msg.items.resize(side_ == BOTH ? 2 : 1); + + if (side_ == LEFT || side_ == BOTH) + { + msg.items[0].id = 16; + msg.items[0].start_angle = deploy_ ? 1 : 2; + msg.items[0].end_angle = deploy_ ? 2 : 1; + msg.items[0].duration_s = 0.5; + } + + if (side_ == RIGHT || side_ == BOTH) + { + msg.items[side_ == BOTH ? 1 : 0].id = 17; + msg.items[side_ == BOTH ? 1 : 0].start_angle = deploy_ ? 1 : 2; + msg.items[side_ == BOTH ? 1 : 0].end_angle = deploy_ ? 2 : 1; + msg.items[side_ == BOTH ? 1 : 0].duration_s = 0.5; + } + + action_executor_->MoveServoTimed(msg); + } + break; + case ActionExec::DONE_STEP: + { + done_ = true; + } + break; + default: + break; + } +} + +void Modelec::ThermoAction::Init(const std::vector& params) +{ + if (!params.empty()) + { + SetSide(static_cast(std::stoi(params[0]))); + SetDeploy(params[1] == "1" || params[1] == "true"); + } +} + +void Modelec::ThermoAction::SetSide(Side side) +{ + side_ = side; +} + +void Modelec::ThermoAction::SetDeploy(bool deploy) +{ + deploy_ = deploy; +} + +void Modelec::ThermoAction::End() +{ + action_executor_->thermo_state_[side_] = deploy_; +} diff --git a/src/modelec_strat/src/action/toggle_servo_action.cpp b/src/modelec_strat/src/action/toggle_servo_action.cpp index 391372b..03778a3 100644 --- a/src/modelec_strat/src/action/toggle_servo_action.cpp +++ b/src/modelec_strat/src/action/toggle_servo_action.cpp @@ -8,17 +8,17 @@ Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, Front front, int n) : ToggleServoAction(action_executor) +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, Side side, int n) : ToggleServoAction(action_executor) { - AddServo(n, front); + AddServo(n, side); } -Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo) : ToggleServoAction(action_executor) +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::pair servo) : ToggleServoAction(action_executor) { AddServo(servo.first, servo.second); } -Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos) : ToggleServoAction(action_executor) +Modelec::ToggleServoAction::ToggleServoAction(const std::shared_ptr& action_executor, std::vector> servos) : ToggleServoAction(action_executor) { AddServos(servos); } @@ -70,23 +70,23 @@ void Modelec::ToggleServoAction::Init(const std::vector& params) for (size_t i = 1; i < params.size(); i += 2) { int id = std::stoi(params[i]); - bool front = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "front") : true; - AddServo(id, front ? FRONT : BACK); + bool side = (i + 1 < params.size()) ? (params[i + 1] == "1" || params[i + 1] == "true" || params[i + 1] == "side") : true; + AddServo(id, side ? FRONT : BACK); } } } -void Modelec::ToggleServoAction::AddServo(int id, Front front) +void Modelec::ToggleServoAction::AddServo(int id, Side side) { - servos_.emplace_back(id, front); + servos_.emplace_back(id, side); } -void Modelec::ToggleServoAction::AddServo(std::pair servo) +void Modelec::ToggleServoAction::AddServo(std::pair servo) { servos_.emplace_back(servo); } -void Modelec::ToggleServoAction::AddServos(const std::vector>& servos) +void Modelec::ToggleServoAction::AddServos(const std::vector>& servos) { servos_.insert(servos_.end(), servos.begin(), servos.end()); } diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index dec71dd..a2466ab 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -8,9 +8,9 @@ Modelec::UPAction::UPAction(const std::shared_ptr& action_execut steps_.push(ActionExec::DONE_STEP); } -Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Front front) : UPAction(action_executor) +Modelec::UPAction::UPAction(const std::shared_ptr& action_executor, Side side) : UPAction(action_executor) { - front_ = front; + side_ = side; } void Modelec::UPAction::Next() @@ -29,9 +29,9 @@ void Modelec::UPAction::Next() case ActionExec::UP_STEP: { ActionServoTimedArray msg; - msg.items.resize(front_ == BOTH ? 8 : 4); + msg.items.resize(side_ == BOTH ? 8 : 4); - if (front_ == FRONT || front_ == BOTH) + if (side_ == FRONT || side_ == BOTH) { msg.items[0].id = 0; msg.items[0].start_angle = 2.91; @@ -54,8 +54,8 @@ void Modelec::UPAction::Next() msg.items[3].duration_s = 0.5; } - if (front_ == BACK || front_ == BOTH) { - int i = front_ == BOTH ? 4 : 0; + if (side_ == BACK || side_ == BOTH) { + int i = side_ == BOTH ? 4 : 0; msg.items[i].id = 8; msg.items[i].start_angle = 0; @@ -95,24 +95,24 @@ void Modelec::UPAction::Init(const std::vector& params) { if (!params.empty()) { - SetFront(static_cast(std::stoi(params[1]))); + SetSide(static_cast(std::stoi(params[1]))); } } -void Modelec::UPAction::SetFront(Front front) +void Modelec::UPAction::SetSide(Side side) { - front_ = front; + side_ = side; } void Modelec::UPAction::End() { - if (front_ == BOTH) + if (side_ == BOTH) { action_executor_->arm_pos_[FRONT].down = false; action_executor_->arm_pos_[BACK].down = false; } else { - action_executor_->arm_pos_[front_].down = false; + action_executor_->arm_pos_[side_].down = false; } } diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 974d65d..42f5553 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -5,6 +5,8 @@ #include "modelec_strat/action/free_action.hpp" #include "modelec_strat/action/take_action.hpp" #include "modelec_strat/action/toggle_servo_action.hpp" +#include "modelec_strat/action/look_on_action.hpp" +#include "modelec_strat/action/thermo_action.hpp" #include "modelec_utils/utils.hpp" namespace Modelec @@ -132,11 +134,11 @@ namespace Modelec } else if (msg->buttons[14] == 1) // LT button { - // IDK + ActivateThermo(BaseAction::Side::LEFT, !thermo_state_[BaseAction::Side::LEFT]); } else if (msg->buttons[15] == 1) // LR button { - // IDK + ActivateThermo(BaseAction::Side::RIGHT, !thermo_state_[BaseAction::Side::RIGHT]); } } if (msg->axes.size() == 8) @@ -265,7 +267,7 @@ namespace Modelec std::swap(action_, empty); } - void ActionExecutor::Down(BaseAction::Front front, bool force, bool inverted) { + void ActionExecutor::Down(BaseAction::Side front, bool force, bool inverted) { if (!arm_pos_[front].down || force) { RCLCPP_DEBUG( @@ -286,16 +288,16 @@ namespace Modelec } } - void ActionExecutor::Up(BaseAction::Front front, bool force) { - if ((arm_pos_[front].down) || force) + void ActionExecutor::Up(BaseAction::Side side, bool force) { + if ((arm_pos_[side].down) || force) { RCLCPP_DEBUG( node_->get_logger(), - "ActionExecutor Up called for front=%d, force=%d", - static_cast(front), + "ActionExecutor Up called for side=%d, force=%d", + static_cast(side), static_cast(force)); - auto action = std::make_shared(shared_from_this(), front); + auto action = std::make_shared(shared_from_this(), side); action_.push(action); if (action_done_) { @@ -307,49 +309,49 @@ namespace Modelec } } - void ActionExecutor::ToggleArm(BaseAction::Front front, bool force) + void ActionExecutor::ToggleArm(BaseAction::Side side, bool force) { RCLCPP_DEBUG( node_->get_logger(), - "ActionExecutor ToggleArm called for front=%d, force=%d", - static_cast(front), + "ActionExecutor ToggleArm called for side=%d, force=%d", + static_cast(side), static_cast(force)); - if (front == BaseAction::BOTH) + if (side == BaseAction::BOTH) { ToggleArm(BaseAction::FRONT, force); ToggleArm(BaseAction::BACK, force); return; } - if (arm_pos_[front].down) + if (arm_pos_[side].down) { - Up(front, force); + Up(side, force); } else { - Down(front, force, arm_pos_[front].rotated); + Down(side, force, arm_pos_[side].rotated); } } - void ActionExecutor::RotateArm(BaseAction::Front front, bool force, bool rotated) + void ActionExecutor::RotateArm(BaseAction::Side side, bool force, bool rotated) { - if (arm_pos_[front].rotated != rotated || force) + if (arm_pos_[side].rotated != rotated || force) { RCLCPP_DEBUG( node_->get_logger(), - "ActionExecutor RotateArm called for front=%d, force=%d, rotated=%d", - static_cast(front), + "ActionExecutor RotateArm called for side=%d, force=%d, rotated=%d", + static_cast(side), static_cast(force), static_cast(rotated)); - if (arm_pos_[front].down) + if (arm_pos_[side].down) { - auto action = std::make_shared(shared_from_this(), front); + auto action = std::make_shared(shared_from_this(), side); action_.push(action); } - auto action = std::make_shared(shared_from_this(), front, rotated); + auto action = std::make_shared(shared_from_this(), side, rotated); action_.push(action); if (action_done_) { @@ -361,7 +363,7 @@ namespace Modelec } } - void ActionExecutor::Take(const std::vector>& servos) { + void ActionExecutor::Take(const std::vector>& servos) { RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Take called for %d servos", @@ -378,7 +380,7 @@ namespace Modelec Update(); } - void ActionExecutor::Free(const std::vector>& servos) { + void ActionExecutor::Free(const std::vector>& servos) { RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor Free called for %d servos", @@ -395,7 +397,7 @@ namespace Modelec Update(); } - void ActionExecutor::ToggleServo(const std::vector>& servos) + void ActionExecutor::ToggleServo(const std::vector>& servos) { RCLCPP_DEBUG( node_->get_logger(), @@ -431,14 +433,36 @@ namespace Modelec step_running_ += msg.items.size(); } - void ActionExecutor::ActivateThermo(int) + void ActionExecutor::ActivateThermo(BaseAction::Side side, bool deploy, bool force) { - // TODO : do something + if (thermo_state_[side] != deploy || force) + { + auto action = std::make_shared(shared_from_this(), side, deploy); + action_.push(action); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + Update(); + } } - void ActionExecutor::DeactivateThermo(int) + void ActionExecutor::LookOn(BaseAction::Side side, bool force) { - // TODO : do something + if (cam_side_ != side || force) + { + auto action = std::make_shared(shared_from_this(), side); + action_.push(action); + if (action_done_) + { + step_running_ = 0; + } + action_done_ = false; + + Update(); + } } void ActionExecutor::ActionFinished(const std::shared_ptr& action) @@ -456,9 +480,9 @@ namespace Modelec return box_obstacles_[0] != nullptr && box_obstacles_[1] != nullptr; } - bool ActionExecutor::HasBox(BaseAction::Front front) const + bool ActionExecutor::HasBox(BaseAction::Side side) const { - return box_obstacles_[front] != nullptr; + return box_obstacles_[side] != nullptr; } bool ActionExecutor::HasFrontBox() const diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 4ee42ae..4fd7277 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -5,8 +5,8 @@ namespace Modelec { FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front) - : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + BaseAction::Side side) + : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -25,7 +25,9 @@ namespace Modelec { steps_.push(GO_TO_FREE); steps_.push(DOWN); - steps_.push(FREE); + steps_.push(FREE_FIRST); + steps_.push(ROTATE_ARM); + steps_.push(FREE_OTHER); steps_.push(UP); steps_.push(GO_BACK); steps_.push(DONE); @@ -86,40 +88,77 @@ namespace Modelec { auto pos = depoPoint.GetTakePosition(200.0); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta += side_ == BaseAction::FRONT ? 0 : M_PI; - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; return; } } + action_executor_->LookOn(BaseAction::Side::CENTER); + go_timeout_ = node_->now(); } break; case DOWN: { - action_executor_->Down(front_); + action_executor_->Down(side_); deploy_time_ = node_->now(); } break; - case FREE: + case FREE_FIRST: + { + auto obs = action_executor_->box_obstacles_[side_]; + + auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::YELLOW : BoxObstacle::BLUE); + + auto servo = std::vector>(); + for (auto s : vect) + { + servo.push_back({s, side_}); + } + + action_executor_->Free(servo); + + deploy_time_ = node_->now(); + + min_time_ = node_->now() + rclcpp::Duration::from_seconds(1); + + if (vect.size() >= 4) + { + std::queue empty; + std::swap(steps_, empty); + + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); + } + } + break; + case ROTATE_ARM: + { + action_executor_->RotateArm(side_, false, true); + deploy_time_ = node_->now(); + } + break; + case FREE_OTHER: { - action_executor_->Free({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); + action_executor_->Free({{0, side_}, {1, side_}, {2, side_}, {3, side_}}); deploy_time_ = node_->now(); - auto obs = action_executor_->box_obstacles_[front_]; - action_executor_->box_obstacles_[front_] = nullptr; + auto obs = action_executor_->box_obstacles_[side_]; + action_executor_->box_obstacles_[side_] = nullptr; auto pos = nav_->GetCurrentPos(); obs->SetPosition( - pos->x + 200 * cos(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), - pos->y + 200 * sin(pos->theta + (front_ == BaseAction::FRONT ? 0 : M_PI)), + pos->x + 200 * cos(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), + pos->y + 200 * sin(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), pos->theta); obs->SetAtObjective(true); @@ -131,7 +170,7 @@ namespace Modelec { break; case UP: { - action_executor_->Up(front_); + action_executor_->Up(side_); deploy_time_ = node_->now(); } break; @@ -142,7 +181,7 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); auto pos = depoPoint.GetTakePosition(300); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta += side_ == BaseAction::FRONT ? 0 : M_PI; if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE) { diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 3647008..fb317e6 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -5,8 +5,8 @@ namespace Modelec { TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor, - BaseAction::Front front) - : front_(front), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) + BaseAction::Side side) + : side_(side), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { } @@ -80,16 +80,16 @@ namespace Modelec { break; } - action_executor_->box_obstacles_[front_] = closestBox; + action_executor_->box_obstacles_[side_] = closestBox; auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - pos.theta += (front_ == BaseAction::FRONT ? 0 : M_PI); + pos.theta += (side_ == BaseAction::FRONT ? 0 : M_PI); - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; @@ -102,16 +102,16 @@ namespace Modelec { break; case GO_TO_TAKE_CLOSE: { - if (action_executor_->box_obstacles_[front_] == nullptr) + if (action_executor_->box_obstacles_[side_] == nullptr) { status_ = MissionStatus::FAILED; break; } - auto pos = action_executor_->box_obstacles_[front_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); - pos.theta += front_ == BaseAction::FRONT ? 0 : M_PI; + auto pos = action_executor_->box_obstacles_[side_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); + pos.theta += side_ == BaseAction::FRONT ? 0 : M_PI; - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, front_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; @@ -122,32 +122,40 @@ namespace Modelec { break; case DOWN: { - action_executor_->Down(front_); + if (action_executor_->arm_pos_[side_].rotated) { + action_executor_->RotateArm(side_, false, false); + } else + { + action_executor_->Down(side_); + } + deploy_time_ = node_->now(); } break; case TAKE: { - action_executor_->Take({{0, front_}, {1, front_}, {2, front_}, {3, front_}}); + action_executor_->Take({{0, side_}, {1, side_}, {2, side_}, {3, side_}}); deploy_time_ = node_->now(); min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); } break; case UP: { - action_executor_->Up(front_); + action_executor_->Up(side_); deploy_time_ = node_->now(); } break; case DONE: { - if (action_executor_->box_obstacles_[front_] == nullptr) + if (action_executor_->box_obstacles_[side_] == nullptr) { status_ = MissionStatus::FAILED; break; } - nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[front_]->GetId()); + nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[side_]->GetId()); + + action_executor_->LookOn(side_); } status_ = MissionStatus::DONE; diff --git a/src/modelec_strat/src/missions/thermo_mission.cpp b/src/modelec_strat/src/missions/thermo_mission.cpp index cb9715c..3492dcd 100644 --- a/src/modelec_strat/src/missions/thermo_mission.cpp +++ b/src/modelec_strat/src/missions/thermo_mission.cpp @@ -101,7 +101,8 @@ namespace Modelec { break; case ACTIVATE_THERMO: { - action_executor_->ActivateThermo(nav_->GetTeamId()); + RCLCPP_INFO(node_->get_logger(), "Activating thermo"); + action_executor_->ActivateThermo(nav_->GetTeamId() == NavigationHelper::YELLOW ? BaseAction::RIGHT : BaseAction::LEFT, true); deploy_time_ = node_->now(); } break; @@ -118,7 +119,8 @@ namespace Modelec { break; case DEACTIVATE_THERMO: { - action_executor_->DeactivateThermo(nav_->GetTeamId()); + RCLCPP_INFO(node_->get_logger(), "Deactivating thermo"); + action_executor_->ActivateThermo(nav_->GetTeamId() == NavigationHelper::YELLOW ? BaseAction::RIGHT : BaseAction::LEFT, false); deploy_time_ = node_->now(); } break; diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 70f6240..64b5e41 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -117,7 +117,7 @@ namespace Modelec return pathfinding_; } - int NavigationHelper::GetTeamId() const + NavigationHelper::Team NavigationHelper::GetTeamId() const { return team_id_; } @@ -719,7 +719,7 @@ namespace Modelec return true; } - void NavigationHelper::SetTeamId(int id) + void NavigationHelper::SetTeamId(Team id) { team_id_ = id; } diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index fdae6b2..118547d 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -57,4 +57,39 @@ namespace Modelec } return positions; } + + void BoxObstacle::SetColor(int index, Team team) + { + if (index >= 0 && index < colors_.size()) + { + colors_[index] = team; + } + } + + BoxObstacle::Team BoxObstacle::GetColor(int index) const + { + if (index >= 0 && index < colors_.size()) + { + return colors_[index]; + } + return YELLOW; + } + + std::array BoxObstacle::GetColors() const + { + return colors_; + } + + std::vector BoxObstacle::GetSide(Team team) const + { + std::vector sideColors; + for (int i = 0; i < colors_.size(); ++i) + { + if (colors_[i] == team) + { + sideColors.push_back(i); + } + } + return sideColors; + } } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 298d0ba..44c2381 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -31,8 +31,7 @@ namespace Modelec team_id_sub_ = create_subscription( "/strat/team", 10, [this](const std_msgs::msg::Int8::SharedPtr msg) { - team_id_ = static_cast(msg->data); - nav_->SetTeamId(team_id_); + nav_->SetTeamId(static_cast(msg->data)); }); spawn_id_sub_ = create_subscription( @@ -40,7 +39,7 @@ namespace Modelec { team_selected_ = true; team_id_ = msg->team_id; - nav_->SetTeamId(team_id_); + nav_->SetTeamId(static_cast(team_id_)); nav_->SetSpawn(msg->name); }); @@ -144,7 +143,7 @@ namespace Modelec arm_msg.data = true; tir_arm_set_pub_->publish(arm_msg); - action_executor_->Up(BaseAction::Front::BOTH, true); + action_executor_->Up(BaseAction::Side::BOTH, true); action_executor_->Free({ {0, BaseAction::FRONT}, {1, BaseAction::FRONT}, {2, BaseAction::FRONT}, {3, BaseAction::FRONT}, {0, BaseAction::BACK}, {1, BaseAction::BACK}, {2, BaseAction::BACK}, {3, BaseAction::BACK}, From cf6ca9f171862eec38a74645b1c442f603614f12 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 2 Feb 2026 19:22:37 +0100 Subject: [PATCH 45/81] fix some rotate things --- src/modelec_strat/src/action_executor.cpp | 2 +- .../src/missions/free_mission.cpp | 36 +++++++++++++------ .../src/missions/take_mission.cpp | 7 +--- 3 files changed, 28 insertions(+), 17 deletions(-) diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 42f5553..93e9c8a 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -336,7 +336,7 @@ namespace Modelec void ActionExecutor::RotateArm(BaseAction::Side side, bool force, bool rotated) { - if (arm_pos_[side].rotated != rotated || force) + if ((arm_pos_[side].rotated != rotated || !arm_pos_[side].down) || force) { RCLCPP_DEBUG( node_->get_logger(), diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 4fd7277..1f99c30 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -99,6 +99,32 @@ namespace Modelec { } } + auto obs = action_executor_->box_obstacles_[side_]; + + auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW); + + if (vect.size() == 4) + { + std::queue empty; + std::swap(steps_, empty); + + steps_.push(DOWN); + steps_.push(FREE_FIRST); + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); + } else if (vect.empty()) + { + std::queue empty; + std::swap(steps_, empty); + + steps_.push(ROTATE_ARM); + steps_.push(FREE_OTHER); + steps_.push(UP); + steps_.push(GO_BACK); + steps_.push(DONE); + } + action_executor_->LookOn(BaseAction::Side::CENTER); go_timeout_ = node_->now(); @@ -128,16 +154,6 @@ namespace Modelec { deploy_time_ = node_->now(); min_time_ = node_->now() + rclcpp::Duration::from_seconds(1); - - if (vect.size() >= 4) - { - std::queue empty; - std::swap(steps_, empty); - - steps_.push(UP); - steps_.push(GO_BACK); - steps_.push(DONE); - } } break; case ROTATE_ARM: diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index fb317e6..e27855d 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -122,12 +122,7 @@ namespace Modelec { break; case DOWN: { - if (action_executor_->arm_pos_[side_].rotated) { - action_executor_->RotateArm(side_, false, false); - } else - { - action_executor_->Down(side_); - } + action_executor_->RotateArm(side_, false, false); deploy_time_ = node_->now(); } From 46cba6432a600264c97bd1fdcc9c0ecbfd6334d2 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 2 Feb 2026 19:54:48 +0100 Subject: [PATCH 46/81] fix some things --- .../modelec_gui/pages/test_map_page.hpp | 21 +- src/modelec_gui/src/pages/test_map_page.cpp | 203 +++++++++++++----- src/modelec_strat/data/config.xml | 8 +- 3 files changed, 168 insertions(+), 64 deletions(-) diff --git a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp index 6e03805..e0a2c2b 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp @@ -54,7 +54,13 @@ namespace ModelecGUI void resizeEvent(QResizeEvent* event) override; - QSvgRenderer* renderer; + void updateBackgroundCache(); + + void updateObstaclesCache(); + + void updateWaypointsCache(); + + QSvgRenderer* renderer_; modelec_interfaces::msg::OdometryPos robotPos; std::vector qpoints; @@ -95,5 +101,18 @@ namespace ModelecGUI bool hasEnemy = false; rclcpp::Publisher::SharedPtr enemy_pos_pub_; rclcpp::Subscription::SharedPtr enemy_pos_sub_; + + QPixmap background_cache_; + QPixmap obstacles_cache_; + QPixmap waypoints_cache_; + + bool bg_dirty_ = true; + bool obstacles_dirty_ = true; + bool waypoints_dirty_ = true; + + QPixmap robot_texture_; + QPixmap top_texture_; + QPixmap obs_texture_; + }; } diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index 8d09f26..fc17e98 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -9,10 +9,14 @@ namespace ModelecGUI { TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), - renderer(new QSvgRenderer( + renderer_(new QSvgRenderer( QString( ":/img/playmat/2026_FINAL.svg"), - this)), node_(node) + this)), + node_(node), + robot_texture_(":/img/logo/modelec.png"), + top_texture_(":/img/logo/ISEN-Nantes.png"), + obs_texture_(":/img/wood.jpg") { ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; @@ -45,6 +49,8 @@ namespace ModelecGUI qpoints.emplace_back(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_); + + waypoints_dirty_ = true; update(); }); @@ -64,6 +70,7 @@ namespace ModelecGUI height() - point.y * ratioBetweenMapAndWidgetY_); } + waypoints_dirty_ = true; update(); }); @@ -85,6 +92,8 @@ namespace ModelecGUI [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.erase(msg->id); + obstacles_dirty_ = true; + update(); }); go_to_pub_ = node_->create_publisher("nav/go_to", 10); @@ -142,13 +151,15 @@ namespace ModelecGUI void TestMapPage::setPlaymatGrid() { - renderer->load(QString(":/img/playmat/grid_v1.svg")); + renderer_->load(QString(":/img/playmat/grid_v1.svg")); + bg_dirty_ = true; update(); } void TestMapPage::setPlaymatMap() { - renderer->load(QString(":/img/playmat/2026_FINAL.svg")); + renderer_->load(QString(":/img/playmat/2026_FINAL.svg")); + bg_dirty_ = true; update(); } @@ -173,73 +184,41 @@ namespace ModelecGUI QWidget::paintEvent(paint_event); QPainter painter(this); - renderer->render(&painter, rect()); // Scales SVG to widget size - painter.save(); - painter.setRenderHint(QPainter::Antialiasing); + if (bg_dirty_) updateBackgroundCache(); + painter.drawPixmap(0, 0, background_cache_); - // --- Draw lines --- - painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide - for (size_t i = 0; i + 1 < qpoints.size(); ++i) + if (show_obstacle_) { - painter.drawLine(qpoints[i], qpoints[i + 1]); + if (obstacles_dirty_) updateObstaclesCache(); + painter.drawPixmap(0, 0, obstacles_cache_); } - painter.setPen(Qt::NoPen); - - // --- Draw colored points --- - const int radius = 5; + if (waypoints_dirty_) updateWaypointsCache(); + painter.drawPixmap(0, 0, waypoints_cache_); - for (size_t i = 0; i < qpoints.size(); ++i) - { - if (i == qpoints.size() - 1) - painter.setBrush(Qt::blue); // Last = blue - else - painter.setBrush(Qt::red); // Middle = red + // --- robot --- + painter.save(); + painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_); + painter.rotate(90 - robotPos.theta * 180.0 / M_PI); - painter.drawEllipse(qpoints[i], radius, radius); - } + QRect r(-(robot_width_*ratioBetweenMapAndWidgetX_)/2, -(robot_length_*ratioBetweenMapAndWidgetY_)/2, + (robot_width_*ratioBetweenMapAndWidgetX_), (robot_length_*ratioBetweenMapAndWidgetY_)); + painter.drawPixmap(r, robot_texture_); painter.restore(); - if (show_obstacle_) + // --- Enemy --- + if (hasEnemy) { - for (auto& [index, obs] : obstacle_) - { - painter.save(); - - QPoint obsPoint(obs.x * ratioBetweenMapAndWidgetX_, height() - obs.y * ratioBetweenMapAndWidgetY_); - painter.translate(obsPoint); - painter.rotate(90 - obs.theta * (180.0 / M_PI)); - painter.setBrush(Qt::black); - - QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), - -(obs.height * ratioBetweenMapAndWidgetY_ / 2), - obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); - - painter.drawRect(toDraw); - - painter.restore(); - } - - // -- Draw enemy position -- - if (hasEnemy) - { - painter.setBrush(Qt::red); - painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, - height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, - enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_); - } + painter.setBrush(Qt::red); + painter.drawRect( + (enemy_pos_.x - enemy_width_/2) * ratioBetweenMapAndWidgetX_, + height() - (enemy_pos_.y + enemy_length_/2) * ratioBetweenMapAndWidgetY_, + enemy_width_ * ratioBetweenMapAndWidgetX_, + enemy_length_ * ratioBetweenMapAndWidgetY_); } - - // -- Draw robot position -- - painter.translate(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_); - painter.rotate(90 - robotPos.theta * (180.0 / M_PI)); - - QRect rect(-(robot_width_ * ratioBetweenMapAndWidgetX_ / 2), -(robot_length_ * ratioBetweenMapAndWidgetY_ / 2), - robot_width_ * ratioBetweenMapAndWidgetX_, robot_length_ * ratioBetweenMapAndWidgetY_); - painter.setBrush(Qt::green); - painter.drawRect(rect); } void TestMapPage::mousePressEvent(QMouseEvent* event) @@ -279,6 +258,8 @@ namespace ModelecGUI void TestMapPage::OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg) { obstacle_.emplace(msg->id, *msg); + obstacles_dirty_ = true; + update(); } void TestMapPage::resizeEvent(QResizeEvent* event) @@ -287,5 +268,109 @@ namespace ModelecGUI ratioBetweenMapAndWidgetX_ = width() / 3000.0f; ratioBetweenMapAndWidgetY_ = height() / 2000.0f; + + bg_dirty_ = true; + obstacles_dirty_ = true; + waypoints_dirty_ = true; + update(); + } + + void TestMapPage::updateBackgroundCache() + { + background_cache_ = QPixmap(size()); + background_cache_.fill(Qt::transparent); + + QPainter p(&background_cache_); + renderer_->render(&p, rect()); + bg_dirty_ = false; + } + + void TestMapPage::updateObstaclesCache() + { + obstacles_cache_ = QPixmap(size()); + obstacles_cache_.fill(Qt::transparent); + + QPainter painter(&obstacles_cache_); + painter.setRenderHint(QPainter::Antialiasing); + + for (auto& [index, obs] : obstacle_) + { + painter.save(); + + QPoint pos(obs.x * ratioBetweenMapAndWidgetX_, + height() - obs.y * ratioBetweenMapAndWidgetY_); + painter.translate(pos); + painter.rotate(90 - obs.theta * 180.0 / M_PI); + + if (obs.type == modelec_interfaces::msg::Obstacle::GRADIN) + { + painter.setBrush(QBrush(obs_texture_)); + } + else if (obs.id == 2) + { + + auto texture = top_texture_.scaled(obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_, Qt::KeepAspectRatio); + + QRect imageRect(-(texture.width() / 2), -(texture.height() / 2), texture.width(), texture.height()); + + QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_); + + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + painter.drawRect(toDraw); + + painter.drawPixmap(imageRect.topLeft(), texture); + + painter.restore(); + + continue; + } + else if (obs.type == modelec_interfaces::msg::Obstacle::ESTRADE) + { + painter.setBrush(Qt::white); + painter.setPen(Qt::NoPen); + } + else + { + painter.setBrush(Qt::red); + painter.setOpacity(0.5); + painter.setPen(QPen(Qt::red, 5)); + } + + QRect r(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), + -(obs.height * ratioBetweenMapAndWidgetY_ / 2), + obs.width * ratioBetweenMapAndWidgetX_, + obs.height * ratioBetweenMapAndWidgetY_); + + painter.drawRect(r); + + painter.restore(); + } + + obstacles_dirty_ = false; + } + + void TestMapPage::updateWaypointsCache() + { + waypoints_cache_ = QPixmap(size()); + waypoints_cache_.fill(Qt::transparent); + + QPainter painter(&waypoints_cache_); + painter.setRenderHint(QPainter::Antialiasing); + painter.setPen(QPen(Qt::red, 2)); + + for (size_t i = 0; i + 1 < qpoints.size(); ++i) + painter.drawLine(qpoints[i], qpoints[i + 1]); + + painter.setPen(Qt::NoPen); + painter.setBrush(Qt::red); + + for (auto& p : qpoints) + painter.drawEllipse(p, 5, 5); + + waypoints_dirty_ = false; } } diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index fd3c30c..cf05bd6 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -80,12 +80,12 @@ 20 - - + + - - + + \ No newline at end of file From 7d43edb3aa9b3d4b69315cafba5e99f9b1315029 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 2 Feb 2026 20:30:52 +0100 Subject: [PATCH 47/81] angle calculation --- .../src/missions/free_mission.cpp | 4 ++-- .../src/missions/take_mission.cpp | 10 ++++----- src/modelec_strat/src/navigation_helper.cpp | 2 +- .../include/modelec_utils/point.hpp | 4 ++++ src/modelec_utils/src/point.cpp | 22 ++++++++++++++++++- 5 files changed, 33 insertions(+), 9 deletions(-) diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 1f99c30..7402c14 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -88,7 +88,7 @@ namespace Modelec { auto pos = depoPoint.GetTakePosition(200.0); - pos.theta += side_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { @@ -197,7 +197,7 @@ namespace Modelec { auto depoPoint = target_deposite_zone_->GetBestTakePosition(Point(currPos->x, currPos->y, currPos->theta)); auto pos = depoPoint.GetTakePosition(300); - pos.theta += side_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); if (nav_->GoTo(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE) != Pathfinding::FREE) { diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index e27855d..9f623ac 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -83,13 +83,13 @@ namespace Modelec { action_executor_->box_obstacles_[side_] = closestBox; auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); - pos.theta += (side_ == BaseAction::FRONT ? 0 : M_PI); + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); - if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE | Pathfinding::WALL, side_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, false, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL, side_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { - if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) + if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { status_ = MissionStatus::FAILED; break; @@ -109,7 +109,7 @@ namespace Modelec { } auto pos = action_executor_->box_obstacles_[side_]->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeClosePosition(); - pos.theta += side_ == BaseAction::FRONT ? 0 : M_PI; + pos.theta = Point::normalizeAngle(pos.theta + (side_ == BaseAction::FRONT ? 0 : M_PI)); if (nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE, side_ == BaseAction::FRONT) != Pathfinding::FREE) { diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 64b5e41..b0e7ac1 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -303,7 +303,7 @@ namespace Modelec { double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); - if (std::abs(angle - (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) + if (Point::angleDiff(angle, (current_pos_->theta + (front ? 0 : M_PI))) > M_PI / 4) { Rotate(angle); return true; diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index ddc4c05..a3d91df 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -15,9 +15,13 @@ namespace Modelec static double distance(const Point& p1, const Point& p2); static double angleDiff(const Point& p1, const Point& p2); + static double angleDiff(const double& p1, const double& p2); double distance(const Point& p2) const; double angleDiff(const Point& p2) const; + double angleDiff(const double& p2) const; + + static double normalizeAngle(double angle); [[nodiscard]] Point GetTakePosition(int distance, double angle) const; [[nodiscard]] Point GetTakePosition(int distance) const; diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index 9580774..d3ffacf 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -9,12 +9,27 @@ namespace Modelec } double Point::angleDiff(const Point &p1, const Point &p2) { - double diff = std::fmod(p1.theta - p2.theta + M_PI, 2 * M_PI); + return angleDiff(p1.theta, p2.theta); + } + + double Point::angleDiff(const double& p1, const double& p2) + { + double diff = std::fmod(p1 - p2 + M_PI, 2 * M_PI); if (diff < 0) diff += 2 * M_PI; return diff - M_PI; } + double Point::normalizeAngle(double angle) + { + double newAngle = std::fmod(angle + M_PI, 2.0 * M_PI); + if (newAngle < 0) { + newAngle += 2.0 * M_PI; + } + + return newAngle - M_PI; + } + double Point::distance(const Point& p2) const { return distance(*this, p2); @@ -25,6 +40,11 @@ namespace Modelec return angleDiff(*this, p2); } + double Point::angleDiff(const double& p2) const + { + return angleDiff(theta, p2); + } + Point Point::GetTakePosition(int distance, double angle) const { Point pos; From f9f2503a71fb48e81eb7f27adda159b4746d8a38 Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 2 Feb 2026 20:48:06 +0100 Subject: [PATCH 48/81] angle calculation --- src/modelec_utils/include/modelec_utils/point.hpp | 2 ++ src/modelec_utils/src/point.cpp | 5 +++++ 2 files changed, 7 insertions(+) diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index a3d91df..083c419 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -21,6 +21,8 @@ namespace Modelec double angleDiff(const Point& p2) const; double angleDiff(const double& p2) const; + void normalizeAngle(); + static double normalizeAngle(double angle); [[nodiscard]] Point GetTakePosition(int distance, double angle) const; diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index d3ffacf..61f3e27 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -45,6 +45,11 @@ namespace Modelec return angleDiff(theta, p2); } + void Point::normalizeAngle() + { + theta = normalizeAngle(theta); + } + Point Point::GetTakePosition(int distance, double angle) const { Point pos; From abdb87b7f429a5e954a8b2516aef0833c444309a Mon Sep 17 00:00:00 2001 From: acki Date: Mon, 2 Feb 2026 21:01:41 +0100 Subject: [PATCH 49/81] type --- .../include/modelec_strat/obstacle/box.hpp | 4 ++-- src/modelec_strat/src/obstacle/box.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index a72aa4a..4ad4d8e 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -26,8 +26,8 @@ namespace Modelec std::vector GetAllPossiblePositions() const; - void SetColor(int index, Team team); - Team GetColor(int index) const; + void SetColor(size_t index, Team team); + Team GetColor(size_t index) const; std::array GetColors() const; std::vector GetSide(Team team) const; diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index 118547d..49c63a8 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -58,17 +58,17 @@ namespace Modelec return positions; } - void BoxObstacle::SetColor(int index, Team team) + void BoxObstacle::SetColor(size_t index, Team team) { - if (index >= 0 && index < colors_.size()) + if (index < colors_.size()) { colors_[index] = team; } } - BoxObstacle::Team BoxObstacle::GetColor(int index) const + BoxObstacle::Team BoxObstacle::GetColor(size_t index) const { - if (index >= 0 && index < colors_.size()) + if (index < colors_.size()) { return colors_[index]; } @@ -83,7 +83,7 @@ namespace Modelec std::vector BoxObstacle::GetSide(Team team) const { std::vector sideColors; - for (int i = 0; i < colors_.size(); ++i) + for (size_t i = 0; i < colors_.size(); ++i) { if (colors_[i] == team) { From 50dcd99ef608cb8e595b9b38b6c858844cabc25f Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:05:21 +0100 Subject: [PATCH 50/81] rota gui --- .../include/modelec_gui/pages/action_page.hpp | 2 ++ src/modelec_gui/src/pages/action_page.cpp | 22 +++++++++++++++++-- src/modelec_strat/data/config.xml | 20 ++--------------- 3 files changed, 24 insertions(+), 20 deletions(-) diff --git a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp index 1d8361f..931534a 100644 --- a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp @@ -61,6 +61,7 @@ namespace ModelecGUI deploy_action1_front_servo4_state_; QPushButton* deploy_action1_front_down_button_; + QPushButton* deploy_action1_front_rotate_button_; // ---- Action1 back ---- QVBoxLayout* deploy_action1_back_layout_; @@ -79,6 +80,7 @@ namespace ModelecGUI deploy_action1_back_servo4_state_; QPushButton* deploy_action1_back_down_button_; + QPushButton* deploy_action1_back_rotate_button_; // ---- Action2 ---- QVBoxLayout* deploy_action2_layout_; diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index 0c48742..b593c75 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -49,6 +49,7 @@ namespace ModelecGUI deploy_action1_front_up_button_ = new QPushButton("Up"); deploy_action1_front_down_button_ = new QPushButton("Down"); + deploy_action1_front_rotate_button_ = new QPushButton("DownRota"); connect(deploy_action1_front_up_button_, &QPushButton::clicked, this, [this]() @@ -62,7 +63,15 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "0"; + action_exec_pub_->publish(action_exec); + }); + + connect(deploy_action1_front_rotate_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -140,6 +149,7 @@ namespace ModelecGUI deploy_action1_back_up_button_ = new QPushButton("Up"); deploy_action1_back_down_button_ = new QPushButton("Down"); + deploy_action1_back_rotate_button_ = new QPushButton("DownRota"); connect(deploy_action1_back_up_button_, &QPushButton::clicked, this, [this]() @@ -153,7 +163,15 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0"; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "0"; + action_exec_pub_->publish(action_exec); + }); + + connect(deploy_action1_back_rotate_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::DOWN + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index cf05bd6..475e3e8 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -12,7 +12,7 @@ 300 50 - -0.3 + -2 @@ -49,29 +49,13 @@ 450 450 - - 300 - 5 - - 20 - 10 - - 4 - 8 - 16 - - - 5 - 5 - 10 - 77 65 - true + false 20 0.6 From 245d656fd917ffd0bddfc569ed7b39251d36aec4 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:09:32 +0100 Subject: [PATCH 51/81] 2s --- src/modelec_strat/src/action/down_action.cpp | 8 ++++---- src/modelec_strat/src/action/up_action.cpp | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index ac3db4f..4f8a90d 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -37,22 +37,22 @@ void Modelec::DownAction::Next() msg.items[0].id = 0; msg.items[0].start_angle = 1.76; msg.items[0].end_angle = 2.91; - msg.items[0].duration_s = 0.5; + msg.items[0].duration_s = 2; msg.items[1].id = 1; msg.items[1].start_angle = 2.06; msg.items[1].end_angle = 0.95; - msg.items[1].duration_s = 0.5; + msg.items[1].duration_s = 2; msg.items[2].id = 2; msg.items[2].start_angle = 0.5; msg.items[2].end_angle = inverted_ ? 0 : 3.2; - msg.items[2].duration_s = 0.5; + msg.items[2].duration_s = 2; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; msg.items[3].end_angle = inverted_ ? 3.1 : 0; - msg.items[3].duration_s = 0.5; + msg.items[3].duration_s = 2; } if (side_ == BACK || side_ == BOTH) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index a2466ab..98f9338 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -36,22 +36,22 @@ void Modelec::UPAction::Next() msg.items[0].id = 0; msg.items[0].start_angle = 2.91; msg.items[0].end_angle = 1.76; - msg.items[0].duration_s = 0.5; + msg.items[0].duration_s = 2; msg.items[1].id = 1; msg.items[1].start_angle = 0.95; msg.items[1].end_angle = 2.06; - msg.items[1].duration_s = 0.5; + msg.items[1].duration_s = 2; msg.items[2].id = 2; msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2; msg.items[2].end_angle = 0.5; - msg.items[2].duration_s = 0.5; + msg.items[2].duration_s = 2; msg.items[3].id = 3; msg.items[3].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 3.1 : 0; msg.items[3].end_angle = 2.6; - msg.items[3].duration_s = 0.5; + msg.items[3].duration_s = 2; } if (side_ == BACK || side_ == BOTH) { From dee4fb5caf6c0dd0caab37b8982fbdb3e3edde2a Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:13:21 +0100 Subject: [PATCH 52/81] middleware --- fastdds_setup.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fastdds_setup.xml b/fastdds_setup.xml index bc22a6e..42dde2c 100644 --- a/fastdds_setup.xml +++ b/fastdds_setup.xml @@ -28,7 +28,7 @@ -
100.73.69.106
+
100.91.204.77
 From 72d4d7be805bf2dc785f4cfecae88122a86903fe Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:15:06 +0100 Subject: [PATCH 53/81] gui --- src/modelec_gui/src/pages/action_page.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index b593c75..35e11fc 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -143,6 +143,7 @@ namespace ModelecGUI deploy_action1_front_layout_->addWidget(deploy_action1_front_up_button_); deploy_action1_front_layout_->addLayout(deploy_action1_front_servos_layout_); deploy_action1_front_layout_->addWidget(deploy_action1_front_down_button_); + deploy_action1_front_layout_->addWidget(deploy_action1_front_rotate_button_); deploy_action1_back_layout_ = new QVBoxLayout; deploy_action1_back_label_ = new QLabel("Back Action 1"); @@ -243,6 +244,7 @@ namespace ModelecGUI deploy_action1_back_layout_->addWidget(deploy_action1_back_up_button_); deploy_action1_back_layout_->addLayout(deploy_action1_back_servos_layout_); deploy_action1_back_layout_->addWidget(deploy_action1_back_down_button_); + deploy_action1_back_layout_->addWidget(deploy_action1_back_rotate_button_); deploy_action1_layout_->addLayout(deploy_action1_front_layout_); deploy_action1_layout_->addLayout(deploy_action1_back_layout_); From 94a3f4b6042c1c47fd68d38670ca18ea6dd0e429 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:19:57 +0100 Subject: [PATCH 54/81] Desktop --- Desktop/no-gui.ros2_launch_marcel.desktop | 8 ++++++++ 1 file changed, 8 insertions(+) create mode 100644 Desktop/no-gui.ros2_launch_marcel.desktop diff --git a/Desktop/no-gui.ros2_launch_marcel.desktop b/Desktop/no-gui.ros2_launch_marcel.desktop new file mode 100644 index 0000000..e9a6bcc --- /dev/null +++ b/Desktop/no-gui.ros2_launch_marcel.desktop @@ -0,0 +1,8 @@ +[Desktop Entry] +Type=Application +Name=Joy +Comment=Lance le système ROS 2 avec GUI +Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_gui:=false +Icon=utilities-terminal +Terminal=true +Categories=Utility; From 1a32862d5946c4d88402558182061641b59bb376 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:21:49 +0100 Subject: [PATCH 55/81] Desktop --- install.sh | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/install.sh b/install.sh index 75a8c44..7cffb6b 100755 --- a/install.sh +++ b/install.sh @@ -44,4 +44,5 @@ chmod +x ~/Desktop/*.desktop gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" true gio set ~/Desktop/no_lidar.ros2_launch_marcel.desktop "metadata::trusted" true gio set ~/Desktop/joy.ros2_launch_marcel.desktop "metadata::trusted" true -gio set ~/Desktop/ros2_launch_marcel.desktop "metadata::trusted" true \ No newline at end of file +gio set ~/Desktop/ros2_launch_marcel.desktop "metadata::trusted" true +gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true \ No newline at end of file From 1495f89591a1888ae3036e93b7afaed527a162a4 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:22:15 +0100 Subject: [PATCH 56/81] Desktop --- Desktop/no-gui.ros2_launch_marcel.desktop | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Desktop/no-gui.ros2_launch_marcel.desktop b/Desktop/no-gui.ros2_launch_marcel.desktop index e9a6bcc..23aa910 100644 --- a/Desktop/no-gui.ros2_launch_marcel.desktop +++ b/Desktop/no-gui.ros2_launch_marcel.desktop @@ -1,7 +1,7 @@ [Desktop Entry] Type=Application -Name=Joy -Comment=Lance le système ROS 2 avec GUI +Name=No GUI +Comment=Lance le système ROS 2 Sans GUI Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_gui:=false Icon=utilities-terminal Terminal=true From b50a735f609b63d4cd5fdf63a01470d4da360be5 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:28:34 +0100 Subject: [PATCH 57/81] not enough internet for fastdds --- start_ros2.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/start_ros2.sh b/start_ros2.sh index 25ed44d..d8fcb39 100755 --- a/start_ros2.sh +++ b/start_ros2.sh @@ -5,8 +5,8 @@ source /opt/ros/jazzy/setup.bash source /home/modelec/Modelec-ROS2/install/setup.bash export RCL_LOG_LEVEL=info -export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +#export RMW_IMPLEMENTATION=rmw_fastrtps_cpp export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml -export ROS_DOMAIN_ID=128 +#export ROS_DOMAIN_ID=128 exec ros2 launch modelec_core modelec.launch.py "$@" From fd552d52d6d8067f482c623c55ea4ae5239f513b Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:32:00 +0100 Subject: [PATCH 58/81] box type --- src/modelec_strat/include/modelec_strat/obstacle/box.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 4ad4d8e..51ff019 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -38,9 +38,9 @@ namespace Modelec std::array colors_ = { YELLOW, - BLUE, YELLOW, - BLUE + YELLOW, + YELLOW }; }; } From c5d0709c35676735ab64076a68c0c5cc3dae8cc0 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:36:51 +0100 Subject: [PATCH 59/81] log --- src/modelec_strat/src/action/up_action.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 98f9338..65dbadb 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -43,6 +43,8 @@ void Modelec::UPAction::Next() msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 2; + RCLCPP_INFO(rclcpp::get_logger("test"), "Front arm rotated: %d", action_executor_->arm_pos_[FRONT].rotated); + msg.items[2].id = 2; msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2; msg.items[2].end_angle = 0.5; From 4fbcdd641646498193b1968638dcc6f415e0e450 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:40:19 +0100 Subject: [PATCH 60/81] log --- src/modelec_strat/src/action/down_action.cpp | 1 + src/modelec_strat/src/action_executor.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 4f8a90d..38a80d2 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -123,6 +123,7 @@ void Modelec::DownAction::End() } else { + RCLCPP_INFO(rclcpp::get_logger("test"), "Setting down state for side %d", side_); action_executor_->arm_pos_[side_].down = true; action_executor_->arm_pos_[side_].rotated = inverted_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 93e9c8a..866ab77 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -338,7 +338,7 @@ namespace Modelec { if ((arm_pos_[side].rotated != rotated || !arm_pos_[side].down) || force) { - RCLCPP_DEBUG( + RCLCPP_INFO( node_->get_logger(), "ActionExecutor RotateArm called for side=%d, force=%d, rotated=%d", static_cast(side), @@ -347,6 +347,9 @@ namespace Modelec if (arm_pos_[side].down) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor RotateArm: arm is down, lifting first"); auto action = std::make_shared(shared_from_this(), side); action_.push(action); } @@ -467,6 +470,9 @@ namespace Modelec void ActionExecutor::ActionFinished(const std::shared_ptr& action) { + RCLCPP_INFO( + node_->get_logger(), + "ActionExecutor ActionFinished called for action"); action->End(); } From 603bb1b3c3a75258181307289b03ca0e4f15c7f0 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:43:27 +0100 Subject: [PATCH 61/81] log --- src/modelec_strat/src/action/down_action.cpp | 1 - src/modelec_strat/src/action/up_action.cpp | 2 -- src/modelec_strat/src/action_executor.cpp | 7 ++----- 3 files changed, 2 insertions(+), 8 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 38a80d2..4f8a90d 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -123,7 +123,6 @@ void Modelec::DownAction::End() } else { - RCLCPP_INFO(rclcpp::get_logger("test"), "Setting down state for side %d", side_); action_executor_->arm_pos_[side_].down = true; action_executor_->arm_pos_[side_].rotated = inverted_; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 65dbadb..98f9338 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -43,8 +43,6 @@ void Modelec::UPAction::Next() msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 2; - RCLCPP_INFO(rclcpp::get_logger("test"), "Front arm rotated: %d", action_executor_->arm_pos_[FRONT].rotated); - msg.items[2].id = 2; msg.items[2].start_angle = action_executor_->arm_pos_[FRONT].rotated ? 0 : 3.2; msg.items[2].end_angle = 0.5; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 866ab77..23b0cdb 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -338,7 +338,7 @@ namespace Modelec { if ((arm_pos_[side].rotated != rotated || !arm_pos_[side].down) || force) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor RotateArm called for side=%d, force=%d, rotated=%d", static_cast(side), @@ -347,9 +347,6 @@ namespace Modelec if (arm_pos_[side].down) { - RCLCPP_INFO( - node_->get_logger(), - "ActionExecutor RotateArm: arm is down, lifting first"); auto action = std::make_shared(shared_from_this(), side); action_.push(action); } @@ -470,7 +467,7 @@ namespace Modelec void ActionExecutor::ActionFinished(const std::shared_ptr& action) { - RCLCPP_INFO( + RCLCPP_DEBUG( node_->get_logger(), "ActionExecutor ActionFinished called for action"); action->End(); From 54346bcc1f96267be8cf35d5ae90e9a291f9cd24 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:53:36 +0100 Subject: [PATCH 62/81] log --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index 4f8a90d..b17e2e8 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -47,12 +47,12 @@ void Modelec::DownAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 0.5; msg.items[2].end_angle = inverted_ ? 0 : 3.2; - msg.items[2].duration_s = 2; + msg.items[2].duration_s = 1; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; msg.items[3].end_angle = inverted_ ? 3.1 : 0; - msg.items[3].duration_s = 2; + msg.items[3].duration_s = 1; } if (side_ == BACK || side_ == BOTH) From 2eb04227ba2bb5b1299a4557dda34eb1d5ac3f59 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:55:44 +0100 Subject: [PATCH 63/81] time --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index b17e2e8..e2f0e7c 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -47,12 +47,12 @@ void Modelec::DownAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 0.5; msg.items[2].end_angle = inverted_ ? 0 : 3.2; - msg.items[2].duration_s = 1; + msg.items[2].duration_s = 1.5; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; msg.items[3].end_angle = inverted_ ? 3.1 : 0; - msg.items[3].duration_s = 1; + msg.items[3].duration_s = 1.5; } if (side_ == BACK || side_ == BOTH) From 871f8a1e4c6fc96e23d89639af96c6bb34de2328 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 19:58:12 +0100 Subject: [PATCH 64/81] time --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index e2f0e7c..c35a056 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -47,12 +47,12 @@ void Modelec::DownAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 0.5; msg.items[2].end_angle = inverted_ ? 0 : 3.2; - msg.items[2].duration_s = 1.5; + msg.items[2].duration_s = 1.7; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; msg.items[3].end_angle = inverted_ ? 3.1 : 0; - msg.items[3].duration_s = 1.5; + msg.items[3].duration_s = 1.7; } if (side_ == BACK || side_ == BOTH) From 768d964ba5ac8dafb0ce3cd8c606d1598353cfef Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 20:05:14 +0100 Subject: [PATCH 65/81] servo page --- .../include/modelec_gui/pages/action_page.hpp | 10 ----- src/modelec_gui/src/pages/action_page.cpp | 40 ++++--------------- 2 files changed, 8 insertions(+), 42 deletions(-) diff --git a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp index 931534a..83e6f6c 100644 --- a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp @@ -55,11 +55,6 @@ namespace ModelecGUI *deploy_action1_front_servo3_button_, *deploy_action1_front_servo4_button_; - bool deploy_action1_front_servo1_state_, - deploy_action1_front_servo2_state_, - deploy_action1_front_servo3_state_, - deploy_action1_front_servo4_state_; - QPushButton* deploy_action1_front_down_button_; QPushButton* deploy_action1_front_rotate_button_; @@ -74,11 +69,6 @@ namespace ModelecGUI *deploy_action1_back_servo3_button_, *deploy_action1_back_servo4_button_; - bool deploy_action1_back_servo1_state_, - deploy_action1_back_servo2_state_, - deploy_action1_back_servo3_state_, - deploy_action1_back_servo4_state_; - QPushButton* deploy_action1_back_down_button_; QPushButton* deploy_action1_back_rotate_button_; diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index 35e11fc..372809a 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -85,12 +85,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo1_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo1_state_ = !deploy_action1_front_servo1_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo1_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -98,12 +95,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo2_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo2_state_ = !deploy_action1_front_servo2_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo2_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "2"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -111,12 +105,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo3_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo3_state_ = !deploy_action1_front_servo3_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo3_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "3"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -124,12 +115,9 @@ namespace ModelecGUI connect(deploy_action1_front_servo4_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_front_servo4_state_ = !deploy_action1_front_servo4_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_front_servo4_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "4"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "4" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -186,12 +174,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo1_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo1_state_ = !deploy_action1_back_servo1_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo1_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -199,12 +184,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo2_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo2_state_ = !deploy_action1_back_servo2_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo2_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "2"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "1" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -212,12 +194,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo3_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo3_state_ = !deploy_action1_back_servo3_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo3_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "3"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "2" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); @@ -225,12 +204,9 @@ namespace ModelecGUI connect(deploy_action1_back_servo4_button_, &QPushButton::clicked, this, [this]() { - deploy_action1_back_servo4_state_ = !deploy_action1_back_servo4_state_; - ActionExec action_exec; - action_exec.action = (deploy_action1_back_servo4_state_ - ? ActionExec::TAKE : ActionExec::FREE) + ActionExec::DELIMITER + "0" + ActionExec::DELIMITER + "4"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "0"; action_exec_pub_->publish(action_exec); }); From d8821f65b570f2c3852395777b2988e2cfe7b54c Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 20:07:28 +0100 Subject: [PATCH 66/81] servo page --- src/modelec_gui/src/pages/action_page.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index 372809a..3c0c88d 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -107,7 +107,7 @@ namespace ModelecGUI { ActionExec action_exec; - action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "2" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); @@ -117,7 +117,7 @@ namespace ModelecGUI { ActionExec action_exec; - action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "4" + ActionExec::DELIMITER + "1"; + action_exec.action = ActionExec::TOGGLE_SERVO + ActionExec::DELIMITER + "3" + ActionExec::DELIMITER + "1"; action_exec_pub_->publish(action_exec); }); From 8933a2acfe140cee3f8230ccf73ec6bb972a86bd Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 20:49:00 +0100 Subject: [PATCH 67/81] please --- src/modelec_strat/include/modelec_strat/obstacle/box.hpp | 8 ++++---- src/modelec_strat/src/action/down_action.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 51ff019..9ba4c4a 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -37,10 +37,10 @@ namespace Modelec std::vector possible_angles_; std::array colors_ = { - YELLOW, - YELLOW, - YELLOW, - YELLOW + BLUE, + BLUE, + BLUE, + BLUE }; }; } diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index c35a056..b677923 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -46,12 +46,12 @@ void Modelec::DownAction::Next() msg.items[2].id = 2; msg.items[2].start_angle = 0.5; - msg.items[2].end_angle = inverted_ ? 0 : 3.2; + msg.items[2].end_angle = inverted_ ? 0 : 3.05; msg.items[2].duration_s = 1.7; msg.items[3].id = 3; msg.items[3].start_angle = 2.6; - msg.items[3].end_angle = inverted_ ? 3.1 : 0; + msg.items[3].end_angle = inverted_ ? 3.1 : 0.3; msg.items[3].duration_s = 1.7; } From b8bf6c89c4e3b583b78c9b955fbff6f573ed7155 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 20:49:39 +0100 Subject: [PATCH 68/81] please --- src/modelec_strat/src/action/down_action.cpp | 4 ++-- src/modelec_strat/src/action/up_action.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_strat/src/action/down_action.cpp b/src/modelec_strat/src/action/down_action.cpp index b677923..f8a93a3 100644 --- a/src/modelec_strat/src/action/down_action.cpp +++ b/src/modelec_strat/src/action/down_action.cpp @@ -36,12 +36,12 @@ void Modelec::DownAction::Next() { msg.items[0].id = 0; msg.items[0].start_angle = 1.76; - msg.items[0].end_angle = 2.91; + msg.items[0].end_angle = 2.93; msg.items[0].duration_s = 2; msg.items[1].id = 1; msg.items[1].start_angle = 2.06; - msg.items[1].end_angle = 0.95; + msg.items[1].end_angle = 0.91; msg.items[1].duration_s = 2; msg.items[2].id = 2; diff --git a/src/modelec_strat/src/action/up_action.cpp b/src/modelec_strat/src/action/up_action.cpp index 98f9338..bef7095 100644 --- a/src/modelec_strat/src/action/up_action.cpp +++ b/src/modelec_strat/src/action/up_action.cpp @@ -34,12 +34,12 @@ void Modelec::UPAction::Next() if (side_ == FRONT || side_ == BOTH) { msg.items[0].id = 0; - msg.items[0].start_angle = 2.91; + msg.items[0].start_angle = 2.93; msg.items[0].end_angle = 1.76; msg.items[0].duration_s = 2; msg.items[1].id = 1; - msg.items[1].start_angle = 0.95; + msg.items[1].start_angle = 0.91; msg.items[1].end_angle = 2.06; msg.items[1].duration_s = 2; From 94cf1db4c6800ae78d3e8e2f26d7fbaeeea51a02 Mon Sep 17 00:00:00 2001 From: acki Date: Wed, 4 Feb 2026 20:55:25 +0100 Subject: [PATCH 69/81] default --- src/modelec_com/src/pcb_action_interface.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 4064dc4..0b426ff 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -263,10 +263,10 @@ namespace Modelec };*/ servo_value_ = { - {0, 2.91}, - {1, 0.95}, - {2, 3.2}, - {3, 0}, + {0, 2.93}, + {1, 0.91}, + {2, 3.05}, + {3, 0.3}, {4, 1}, {5, 1}, {6, 1}, From 6e6a2c44030d694a1836bebfcae109305ab9db18 Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 5 Feb 2026 23:00:57 +0100 Subject: [PATCH 70/81] setup cam backup --- build.zsh | 2 +- compilation_database.build.zsh | 3 - simulated_pcb/odo.py | 3 + src/modelec_com/CMakeLists.txt | 28 ++++ .../include/modelec_com/color_detector.hpp | 34 ++++ src/modelec_com/src/color_detector.cpp | 153 ++++++++++++++++++ src/modelec_core/launch/modelec.launch.py | 21 +++ .../include/modelec_strat/action_executor.hpp | 9 ++ .../include/modelec_strat/obstacle/box.hpp | 2 + src/modelec_strat/src/action_executor.cpp | 53 ++++++ .../src/missions/free_mission.cpp | 33 ++-- .../src/missions/take_mission.cpp | 3 +- src/modelec_strat/src/obstacle/box.cpp | 18 +++ start_ros2.sh | 2 +- 14 files changed, 342 insertions(+), 22 deletions(-) delete mode 100755 compilation_database.build.zsh create mode 100644 src/modelec_com/include/modelec_com/color_detector.hpp create mode 100644 src/modelec_com/src/color_detector.cpp diff --git a/build.zsh b/build.zsh index bfb17da..fd96718 100755 --- a/build.zsh +++ b/build.zsh @@ -1,3 +1,3 @@ -colcon build # --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug +colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug source "install/setup.zsh" diff --git a/compilation_database.build.zsh b/compilation_database.build.zsh deleted file mode 100755 index c340a78..0000000 --- a/compilation_database.build.zsh +++ /dev/null @@ -1,3 +0,0 @@ -colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON # -DCMAKE_BUILD_TYPE=Debug - -source "install/setup.zsh" diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index 6aefd4e..2be80ea 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -125,6 +125,9 @@ def handle_message(self, msg): elif msg.startswith("SET;START;"): self.start = msg.split(';')[2] == '1' + if not self.start: + self.waypoints.clear() + self.waypoint_queue.clear() self.ser.write(b"OK;START;1\n") # --- MULTI WAYPOINT (clears previous) --- diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index 5e16b00..329f175 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -10,10 +10,13 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(std_srvs REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(ament_index_cpp REQUIRED) find_package(fmt) +find_package(OpenCV REQUIRED) + find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) @@ -35,10 +38,35 @@ target_include_directories(pcb_action_interface PUBLIC $ ) +# Color Detector Node +add_executable(color_detector + src/color_detector.cpp +) + +ament_target_dependencies(color_detector + rclcpp + std_msgs + std_srvs + modelec_interfaces + ament_index_cpp +) + +target_link_libraries(color_detector + ${OpenCV_LIBS} + modelec_utils::utils + modelec_utils::config +) + +target_include_directories(color_detector PUBLIC + $ + $ +) + # Install targets install(TARGETS pcb_odo_interface pcb_action_interface + color_detector DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp new file mode 100644 index 0000000..2ba3154 --- /dev/null +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -0,0 +1,34 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace Modelec +{ + class ColorDetector : public rclcpp::Node + { + public: + ColorDetector(); + private: + void onRequest( + const std::shared_ptr request, + std::shared_ptr response); + + std::vector classifyROIs(const cv::Mat& hsv) const; + + std::string classify(const cv::Vec3d& hsv) const; + + std::string generateImagePath() const; + + cv::VideoCapture cap_; + rclcpp::Service::SharedPtr service_; + + rclcpp::Subscription::SharedPtr ask_sub_; + rclcpp::Publisher::SharedPtr color_pub_; + + bool save_to_file_ = true; + }; +} diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp new file mode 100644 index 0000000..da922e6 --- /dev/null +++ b/src/modelec_com/src/color_detector.cpp @@ -0,0 +1,153 @@ +#include + +#include "modelec_utils/utils.hpp" + +namespace Modelec +{ + ColorDetector::ColorDetector() + : Node("color_detector") + { + cap_.open("http://192.168.1.21:8080/video"); + if (!cap_.isOpened()) { + RCLCPP_FATAL(get_logger(), "Camera not detected"); + rclcpp::shutdown(); + return; + } + + service_ = create_service( + "action/detect_color", + std::bind(&ColorDetector::onRequest, this, + std::placeholders::_1, + std::placeholders::_2)); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); + + ask_sub_ = create_subscription( + "action/detect_color/ask", qos, + [this](const std_msgs::msg::Empty::SharedPtr) + { + auto res = std_msgs::msg::String(); + + if (!cap_.isOpened()) + { + res.data = "0;Camera not opened"; + return; + } + + cv::Mat frame; + cap_ >> frame; + + if (frame.empty()) { + res.data = "0;Empty frame"; + return; + } + + if (save_to_file_) + { + std::string path = generateImagePath(); + cv::imwrite(path, frame); + RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); + } + + cv::Mat hsv; + cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); + + auto colors = classifyROIs(hsv); + + res.data = "1;" + join(colors, ";"); + + color_pub_->publish(res); + }); + + color_pub_ = create_publisher("action/detect_color/res", qos); + + RCLCPP_INFO(get_logger(), "Color detector service ready"); + } + + void ColorDetector::onRequest( + const std::shared_ptr, + std::shared_ptr response) + { + cv::Mat frame; + cap_ >> frame; + + if (frame.empty()) { + response->success = false; + response->message = "Empty frame"; + return; + } + + if (save_to_file_) + { + std::string path = generateImagePath(); + cv::imwrite(path, frame); + RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); + } + + cv::Mat hsv; + cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); + + auto colors = classifyROIs(hsv); + + response->success = true; + response->message = join(colors, ";"); + } + + // 4 independent ROIs + std::vector ColorDetector::classifyROIs(const cv::Mat& hsv) const + { + std::vector rois = { + { 98, 98, 5, 5}, + {198, 98, 5, 5}, + { 98, 198, 5, 5}, + {198, 198, 5, 5} + }; + + std::vector results; + for (const auto& roi : rois) { + cv::Rect safe_roi = roi & cv::Rect(0, 0, hsv.cols, hsv.rows); + cv::Scalar mean = cv::mean(hsv(safe_roi)); + results.push_back(classify(cv::Vec3d(mean[0], mean[1], mean[2]))); + } + return results; + } + + std::string ColorDetector::classify(const cv::Vec3d& hsv) const + { + int h = static_cast(hsv[0]); + int s = static_cast(hsv[1]); + int v = static_cast(hsv[2]); + + RCLCPP_DEBUG(get_logger(), "Classifying color with HSV: (%d, %d, %d)", h, s, v); + + if (s < 50 || v < 50) + return "unknown"; + + if (h >= 60) + return "blue"; + return "yellow"; + } + + std::string ColorDetector::generateImagePath() const + { + auto now = std::chrono::system_clock::now(); + auto in_time_t = std::chrono::system_clock::to_time_t(now); + std::stringstream ss; + ss << "snapshot_" + << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S") + << ".png"; // or .jpg + return ss.str(); + } + +} + + +int main(int argc, char* argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index fbc7dbb..7d55ca7 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -36,6 +36,9 @@ def generate_launch_description(): with_joy_arg = DeclareLaunchArgument( 'with_joy', default_value='true', description='Launch joystick node?' ) + with_color_detector_arg = DeclareLaunchArgument( + 'with_color_detector', default_value='true', description='Launch color detector node?' + ) channel_type = LaunchConfiguration('channel_type', default='serial') serial_port = LaunchConfiguration('serial_port', default='/dev/rplidar') @@ -153,6 +156,8 @@ def launch_strat(context, *args, **kwargs): package='modelec_strat', executable='strat_fsm', name='strat_fsm', + # prefix=['xterm -e gdb -ex run --args'], + # output='screen', ), Node( package='modelec_strat', @@ -191,6 +196,18 @@ def launch_joy(context, *args, **kwargs): ] return [] + def launch_color_detector(context, *args, **kwargs): + if context.launch_configurations.get('with_color_detector') == 'true': + return [ + Node( + package='modelec_com', + executable='color_detector', + name='color_detector', + output='screen', + ) + ] + return [] + # ------------------------------------------------- # Final LaunchDescription # ------------------------------------------------- @@ -209,6 +226,7 @@ def launch_joy(context, *args, **kwargs): with_strat_arg, with_enemy_manager_arg, with_joy_arg, + with_color_detector_arg, OpaqueFunction(function=launch_rplidar_restart_if_needed), OpaqueFunction(function=launch_gui), @@ -216,4 +234,7 @@ def launch_joy(context, *args, **kwargs): OpaqueFunction(function=launch_strat), OpaqueFunction(function=launch_enemy_manager), OpaqueFunction(function=launch_joy), + OpaqueFunction(function=launch_color_detector), ]) + +# prefix = ['xterm -e gdb -ex run --args'] \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 9f633ed..becc6fc 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -9,6 +9,8 @@ #include #include +#include +#include #include "action/base_action.hpp" #include "missions/thermo_mission.hpp" @@ -102,6 +104,8 @@ namespace Modelec void SendPoint(int point) const; + void AskColor() const; + protected: rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Publisher::SharedPtr servo_move_pub_; @@ -113,11 +117,16 @@ namespace Modelec rclcpp::Subscription::SharedPtr relay_move_res_sub_; rclcpp::Subscription::SharedPtr servo_timed_move_res_sub_; + rclcpp::Client::SharedPtr ask_color_client_; + rclcpp::Subscription::SharedPtr action_exec_sub_; rclcpp::Subscription::SharedPtr joy_sub_; rclcpp::Publisher::SharedPtr score_pub_; + rclcpp::Publisher::SharedPtr ask_pub_; + rclcpp::Subscription::SharedPtr color_sub_; + std::queue> action_; bool action_done_ = true; diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 9ba4c4a..771822c 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -32,6 +32,8 @@ namespace Modelec std::vector GetSide(Team team) const; + void ParseColor(const std::string& colorStr); + protected: std::vector possible_angles_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 23b0cdb..3f36894 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -205,6 +205,33 @@ namespace Modelec }); score_pub_ = node_->create_publisher("/strat/score", 10); + + ask_color_client_ = node_->create_client("action/detect_color"); + + rclcpp::QoS qos(rclcpp::KeepLast(10)); + qos.reliable(); + + color_sub_ = node_->create_subscription( + "/action/detect_color/res", qos, [this](const std_msgs::msg::String::SharedPtr msg) + { + auto res = split(msg->data, ';'); + if (res.size() < 2 || res[0] != "1") + { + RCLCPP_WARN(node_->get_logger(), "Color detection failed: %s", msg->data.c_str()); + return; + } + + RCLCPP_INFO(node_->get_logger(), "Color detection succeeded: %s", res[1].c_str()); + + if (box_obstacles_[cam_side_] != nullptr) { + box_obstacles_[cam_side_]->ParseColor(res[1]); + } else { + RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); + } + }); + + ask_pub_ = node_->create_publisher( + "/action/detect_color/ask", qos); } rclcpp::Node::SharedPtr ActionExecutor::GetNode() const @@ -509,4 +536,30 @@ namespace Modelec msg.data = point; score_pub_->publish(msg); } + + void ActionExecutor::AskColor() const + { + /*if (!ask_color_client_->service_is_ready()) { + RCLCPP_ERROR(node_->get_logger(), "Color detection service not ready"); + return; + } + + auto request = std::make_shared(); + + ask_color_client_->async_send_request(request, + [this](rclcpp::Client::SharedFuture future) { + auto response = future.get(); + if (response->success) { + RCLCPP_INFO(node_->get_logger(), "Color detection succeeded: %s", response->message.c_str()); + if (box_obstacles_[cam_side_] != nullptr) { + box_obstacles_[cam_side_]->ParseColor(response->message); + } else { + RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); + } + } else { + RCLCPP_ERROR(node_->get_logger(), "Color detection failed: %s", response->message.c_str()); + } + });*/ + ask_pub_->publish(std_msgs::msg::Empty()); + } } diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 7402c14..854c8d4 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -37,6 +37,7 @@ namespace Modelec { { if (!action_executor_->IsActionDone()) { + // RCLCPP_INFO(node_->get_logger(), "FreeMission: Waiting for action to complete"); return; } @@ -141,7 +142,7 @@ namespace Modelec { { auto obs = action_executor_->box_obstacles_[side_]; - auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::YELLOW : BoxObstacle::BLUE); + auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW); auto servo = std::vector>(); for (auto s : vect) @@ -167,21 +168,7 @@ namespace Modelec { action_executor_->Free({{0, side_}, {1, side_}, {2, side_}, {3, side_}}); deploy_time_ = node_->now(); - auto obs = action_executor_->box_obstacles_[side_]; - action_executor_->box_obstacles_[side_] = nullptr; - - auto pos = nav_->GetCurrentPos(); - - obs->SetPosition( - pos->x + 200 * cos(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), - pos->y + 200 * sin(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), - pos->theta); - - obs->SetAtObjective(true); - - nav_->GetPathfinding()->AddObstacle(obs); - - min_time_ = node_->now() + rclcpp::Duration::from_seconds(0.5); + min_time_ = node_->now() + rclcpp::Duration::from_seconds(1); } break; case UP: @@ -211,6 +198,20 @@ namespace Modelec { break; case DONE: { + auto obs = action_executor_->box_obstacles_[side_]; + action_executor_->box_obstacles_[side_] = nullptr; + + auto pos = nav_->GetCurrentPos(); + + obs->SetPosition( + pos->x + 300 * cos(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), + pos->y + 300 * sin(pos->theta + (side_ == BaseAction::FRONT ? 0 : M_PI)), + pos->theta); + + obs->SetAtObjective(true); + + nav_->GetPathfinding()->AddObstacle(obs); + target_deposite_zone_->Validate(true); } diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp index 9f623ac..efe1cab 100644 --- a/src/modelec_strat/src/missions/take_mission.cpp +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -137,6 +137,7 @@ namespace Modelec { case UP: { action_executor_->Up(side_); + action_executor_->LookOn(side_); deploy_time_ = node_->now(); } break; @@ -150,7 +151,7 @@ namespace Modelec { nav_->GetPathfinding()->RemoveObstacle(action_executor_->box_obstacles_[side_]->GetId()); - action_executor_->LookOn(side_); + action_executor_->AskColor(); } status_ = MissionStatus::DONE; diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index 49c63a8..bb3fe74 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -1,5 +1,7 @@ #include +#include "modelec_utils/utils.hpp" + namespace Modelec { BoxObstacle::BoxObstacle(int id, int x, int y, double theta, int w, int h, const std::string& type) @@ -92,4 +94,20 @@ namespace Modelec } return sideColors; } + + void BoxObstacle::ParseColor(const std::string& colorStr) + { + std::vector tokens = split(colorStr, ';'); + for (size_t i = 0; i < tokens.size() && i < colors_.size(); ++i) + { + if (tokens[i] == "yellow") + { + colors_[i] = YELLOW; + } + else if (tokens[i] == "blue") + { + colors_[i] = BLUE; + } + } + } } diff --git a/start_ros2.sh b/start_ros2.sh index d8fcb39..5305644 100755 --- a/start_ros2.sh +++ b/start_ros2.sh @@ -9,4 +9,4 @@ export RCL_LOG_LEVEL=info export FASTRTPS_DEFAULT_PROFILES_FILE=/home/modelec/Modelec-ROS2/fastdds_setup.xml #export ROS_DOMAIN_ID=128 -exec ros2 launch modelec_core modelec.launch.py "$@" +exec ros2 launch modelec_core modelec.launch.py with_color_detector:=false "$@" From dd5248d4f012c097000ac74d6307e4ae2079c60f Mon Sep 17 00:00:00 2001 From: acki Date: Thu, 5 Feb 2026 23:23:59 +0100 Subject: [PATCH 71/81] cam config --- .../include/modelec_com/color_detector.hpp | 3 ++ src/modelec_com/src/color_detector.cpp | 29 +++++++++++++++---- src/modelec_com/src/pcb_action_interface.cpp | 2 -- src/modelec_com/src/pcb_odo_interface.cpp | 2 -- src/modelec_strat/data/config.xml | 8 +++++ 5 files changed, 35 insertions(+), 9 deletions(-) diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp index 2ba3154..18ec3eb 100644 --- a/src/modelec_com/include/modelec_com/color_detector.hpp +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -29,6 +29,9 @@ namespace Modelec rclcpp::Subscription::SharedPtr ask_sub_; rclcpp::Publisher::SharedPtr color_pub_; + std::string link_; bool save_to_file_ = true; + std::string save_directory_ = "./"; + bool enable_ = false; }; } diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index da922e6..6a8aaf5 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -1,13 +1,32 @@ #include - -#include "modelec_utils/utils.hpp" +#include +#include +#include namespace Modelec { ColorDetector::ColorDetector() : Node("color_detector") { - cap_.open("http://192.168.1.21:8080/video"); + std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; + if (!Config::load(config_path)) + { + RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); + } + + link_ = Config::get("config.cam.link", "/dev/video0"); + save_to_file_ = Config::get("config.cam.save_to_file.enabled", false); + save_directory_ = Config::get("config.cam.save_to_file.path", "./"); + enable_ = Config::get("config.cam.enabled", false); + + if (!enable_) + { + RCLCPP_INFO(get_logger(), "Camera disabled by config"); + rclcpp::shutdown(); + return; + } + + cap_.open(link_); if (!cap_.isOpened()) { RCLCPP_FATAL(get_logger(), "Camera not detected"); rclcpp::shutdown(); @@ -45,7 +64,7 @@ namespace Modelec if (save_to_file_) { - std::string path = generateImagePath(); + std::string path = save_directory_ + generateImagePath(); cv::imwrite(path, frame); RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); } @@ -80,7 +99,7 @@ namespace Modelec if (save_to_file_) { - std::string path = generateImagePath(); + std::string path = save_directory_ + generateImagePath(); cv::imwrite(path, frame); RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); } diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 0b426ff..3d1a5e2 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -1,6 +1,4 @@ #include -#include -#include #include #include diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 11a2a96..6f2e9db 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -1,7 +1,5 @@ #include #include -#include -#include namespace Modelec { diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 475e3e8..95335a8 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -72,4 +72,12 @@ + + + true + ./cam/ + + false + /dev/video0 + \ No newline at end of file From daa1fa721673f25bb4b5675dd482da44deaeeeb8 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 6 Feb 2026 13:05:15 +0100 Subject: [PATCH 72/81] arrange code + more debug --- .../include/modelec_com/color_detector.hpp | 4 +- src/modelec_com/src/color_detector.cpp | 104 +++++++++++------- 2 files changed, 66 insertions(+), 42 deletions(-) diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp index 18ec3eb..207789b 100644 --- a/src/modelec_com/include/modelec_com/color_detector.hpp +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -13,11 +13,13 @@ namespace Modelec public: ColorDetector(); private: + bool processSnapshot(std::vector& colors, std::string& error); + void onRequest( const std::shared_ptr request, std::shared_ptr response); - std::vector classifyROIs(const cv::Mat& hsv) const; + std::vector classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const; std::string classify(const cv::Vec3d& hsv) const; diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index 6a8aaf5..fdd36ef 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -46,36 +46,19 @@ namespace Modelec "action/detect_color/ask", qos, [this](const std_msgs::msg::Empty::SharedPtr) { - auto res = std_msgs::msg::String(); + std_msgs::msg::String res; + std::vector colors; + std::string error; - if (!cap_.isOpened()) + if (!processSnapshot(colors, error)) { - res.data = "0;Camera not opened"; - return; + res.data = "0;" + error; } - - cv::Mat frame; - cap_ >> frame; - - if (frame.empty()) { - res.data = "0;Empty frame"; - return; - } - - if (save_to_file_) + else { - std::string path = save_directory_ + generateImagePath(); - cv::imwrite(path, frame); - RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); + res.data = "1;" + join(colors, ";"); } - cv::Mat hsv; - cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); - - auto colors = classifyROIs(hsv); - - res.data = "1;" + join(colors, ";"); - color_pub_->publish(res); }); @@ -84,37 +67,58 @@ namespace Modelec RCLCPP_INFO(get_logger(), "Color detector service ready"); } - void ColorDetector::onRequest( - const std::shared_ptr, - std::shared_ptr response) + bool ColorDetector::processSnapshot(std::vector& colors, std::string& error) { + if (!cap_.isOpened()) + { + error = "Camera not opened"; + return false; + } + cv::Mat frame; cap_ >> frame; - if (frame.empty()) { - response->success = false; - response->message = "Empty frame"; - return; + if (frame.empty()) + { + error = "Empty frame"; + return false; } + cv::Mat hsv; + cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); + + colors = classifyROIs(hsv, frame); + if (save_to_file_) { std::string path = save_directory_ + generateImagePath(); cv::imwrite(path, frame); - RCLCPP_INFO(get_logger(), "Saved snapshot to %s", path.c_str()); + RCLCPP_INFO(get_logger(), "Saved annotated snapshot: %s", path.c_str()); } - cv::Mat hsv; - cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); + return true; + } - auto colors = classifyROIs(hsv); + void ColorDetector::onRequest( + const std::shared_ptr, + std::shared_ptr response) + { + std::vector colors; + std::string error; + + if (!processSnapshot(colors, error)) + { + response->success = false; + response->message = error; + return; + } response->success = true; response->message = join(colors, ";"); } // 4 independent ROIs - std::vector ColorDetector::classifyROIs(const cv::Mat& hsv) const + std::vector ColorDetector::classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const { std::vector rois = { { 98, 98, 5, 5}, @@ -124,11 +128,29 @@ namespace Modelec }; std::vector results; - for (const auto& roi : rois) { - cv::Rect safe_roi = roi & cv::Rect(0, 0, hsv.cols, hsv.rows); - cv::Scalar mean = cv::mean(hsv(safe_roi)); - results.push_back(classify(cv::Vec3d(mean[0], mean[1], mean[2]))); + + for (auto r : rois) + { + cv::Rect roi = r & cv::Rect(0, 0, hsv.cols, hsv.rows); + cv::Scalar mean = cv::mean(hsv(roi)); + + std::string color = classify(cv::Vec3d(mean[0], mean[1], mean[2])); + results.push_back(color); + + if (save_to_file_) + { + cv::rectangle(debug_img, roi, {0, 255, 0}, 1); + cv::putText( + debug_img, + color, + roi.tl() + cv::Point(0, -5), + cv::FONT_HERSHEY_SIMPLEX, + 0.4, + {0, 255, 0}, + 1); + } } + return results; } @@ -155,7 +177,7 @@ namespace Modelec std::stringstream ss; ss << "snapshot_" << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S") - << ".png"; // or .jpg + << ".png"; return ss.str(); } From 772a888f48c621ea83b48cd11d87e8324cbcb8c3 Mon Sep 17 00:00:00 2001 From: acki Date: Fri, 6 Feb 2026 18:10:01 +0100 Subject: [PATCH 73/81] have to reopen cam, not like this but idk how to do without that --- .../include/modelec_com/color_detector.hpp | 10 +- src/modelec_com/src/color_detector.cpp | 112 +++++++++++++----- src/modelec_strat/data/config.xml | 12 +- 3 files changed, 101 insertions(+), 33 deletions(-) diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp index 207789b..53f4908 100644 --- a/src/modelec_com/include/modelec_com/color_detector.hpp +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -12,6 +12,8 @@ namespace Modelec { public: ColorDetector(); + + ~ColorDetector() override; private: bool processSnapshot(std::vector& colors, std::string& error); @@ -21,19 +23,23 @@ namespace Modelec std::vector classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const; - std::string classify(const cv::Vec3d& hsv) const; + std::string classify(const cv::Vec3d& hsv_roi) const; std::string generateImagePath() const; - cv::VideoCapture cap_; + void SetupRois(); + rclcpp::Service::SharedPtr service_; rclcpp::Subscription::SharedPtr ask_sub_; rclcpp::Publisher::SharedPtr color_pub_; + std::vector rois_; + std::string link_; bool save_to_file_ = true; std::string save_directory_ = "./"; bool enable_ = false; + bool headless_ = true; }; } diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index fdd36ef..d046ecc 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -18,6 +18,7 @@ namespace Modelec save_to_file_ = Config::get("config.cam.save_to_file.enabled", false); save_directory_ = Config::get("config.cam.save_to_file.path", "./"); enable_ = Config::get("config.cam.enabled", false); + headless_ = Config::get("config.cam.headless", true); if (!enable_) { @@ -26,13 +27,6 @@ namespace Modelec return; } - cap_.open(link_); - if (!cap_.isOpened()) { - RCLCPP_FATAL(get_logger(), "Camera not detected"); - rclcpp::shutdown(); - return; - } - service_ = create_service( "action/detect_color", std::bind(&ColorDetector::onRequest, this, @@ -64,19 +58,40 @@ namespace Modelec color_pub_ = create_publisher("action/detect_color/res", qos); + SetupRois(); + + if (!headless_) + { + cv::namedWindow("color_detector", cv::WINDOW_NORMAL); + } + RCLCPP_INFO(get_logger(), "Color detector service ready"); } + ColorDetector::~ColorDetector() + { + if (!headless_) + { + cv::destroyAllWindows(); + } + } + bool ColorDetector::processSnapshot(std::vector& colors, std::string& error) { - if (!cap_.isOpened()) + cv::VideoCapture cap(link_); + + if (!cap.isOpened()) { - error = "Camera not opened"; + error = "Failed to open camera"; return false; } + cap.set(cv::CAP_PROP_BUFFERSIZE, 1); + cv::Mat frame; - cap_ >> frame; + + for (int i = 0; i < 3; ++i) + cap >> frame; if (frame.empty()) { @@ -85,17 +100,25 @@ namespace Modelec } cv::Mat hsv; + cv::GaussianBlur(frame, frame, cv::Size(5, 5), 0); cv::cvtColor(frame, hsv, cv::COLOR_BGR2HSV); colors = classifyROIs(hsv, frame); + if (!headless_) + { + cv::imshow("color_detector", frame); + cv::waitKey(1); + } + if (save_to_file_) { std::string path = save_directory_ + generateImagePath(); cv::imwrite(path, frame); - RCLCPP_INFO(get_logger(), "Saved annotated snapshot: %s", path.c_str()); } + cap.release(); + return true; } @@ -120,16 +143,9 @@ namespace Modelec // 4 independent ROIs std::vector ColorDetector::classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const { - std::vector rois = { - { 98, 98, 5, 5}, - {198, 98, 5, 5}, - { 98, 198, 5, 5}, - {198, 198, 5, 5} - }; - std::vector results; - for (auto r : rois) + for (auto r : rois_) { cv::Rect roi = r & cv::Rect(0, 0, hsv.cols, hsv.rows); cv::Scalar mean = cv::mean(hsv(roi)); @@ -154,20 +170,26 @@ namespace Modelec return results; } - std::string ColorDetector::classify(const cv::Vec3d& hsv) const + std::string ColorDetector::classify(const cv::Vec3d& hsv_roi) const { - int h = static_cast(hsv[0]); - int s = static_cast(hsv[1]); - int v = static_cast(hsv[2]); + cv::Scalar yellow_low(20, 100, 100), yellow_high(40, 255, 255); + cv::Scalar blue_low(100, 100, 100), blue_high(130, 255, 255); - RCLCPP_DEBUG(get_logger(), "Classifying color with HSV: (%d, %d, %d)", h, s, v); + cv::Mat yellow_mask, blue_mask; + cv::inRange(hsv_roi, yellow_low, yellow_high, yellow_mask); + cv::inRange(hsv_roi, blue_low, blue_high, blue_mask); - if (s < 50 || v < 50) - return "unknown"; + int yellow_count = cv::countNonZero(yellow_mask); + int blue_count = cv::countNonZero(blue_mask); + int total_pixels = hsv_roi.rows * hsv_roi.cols; - if (h >= 60) - return "blue"; - return "yellow"; + double yellow_ratio = static_cast(yellow_count) / total_pixels; + double blue_ratio = static_cast(blue_count) / total_pixels; + + if (yellow_ratio > blue_ratio && yellow_ratio > 0.3) return "yellow"; + if (blue_ratio > yellow_ratio && blue_ratio > 0.3) return "blue"; + + return "unknown"; } std::string ColorDetector::generateImagePath() const @@ -181,6 +203,38 @@ namespace Modelec return ss.str(); } + void ColorDetector::SetupRois() + { + rois_.resize(4); + + rois_[0] = { + Config::get("config.cam.points.first@x", 0), + Config::get("config.cam.points.first@y", 0), + Config::get("config.cam.points.first@w", 0), + Config::get("config.cam.points.first@h", 0) + }; + + rois_[1] = { + Config::get("config.cam.points.second@x", 0), + Config::get("config.cam.points.second@y", 0), + Config::get("config.cam.points.second@w", 0), + Config::get("config.cam.points.second@h", 0) + }; + + rois_[2] = { + Config::get("config.cam.points.third@x", 0), + Config::get("config.cam.points.third@y", 0), + Config::get("config.cam.points.third@w", 0), + Config::get("config.cam.points.third@h", 0) + }; + + rois_[3] = { + Config::get("config.cam.points.fourth@x", 0), + Config::get("config.cam.points.fourth@y", 0), + Config::get("config.cam.points.fourth@w", 0), + Config::get("config.cam.points.fourth@h", 0) + }; + } } diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 95335a8..3c4c1b5 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -77,7 +77,15 @@ true ./cam/ - false - /dev/video0 + true + + http://192.168.1.21:8080/video + true + + + + + + \ No newline at end of file From 9dfcc83a01e7568ed7a07a19ffca11f46bb3505c Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 11:01:46 +0100 Subject: [PATCH 74/81] classify + config array --- src/modelec_com/src/color_detector.cpp | 75 ++++++++---------- src/modelec_strat/data/config.xml | 12 +-- .../include/modelec_utils/config.hpp | 6 +- src/modelec_utils/src/config.cpp | 77 +++++++++++++++---- 4 files changed, 106 insertions(+), 64 deletions(-) diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index d046ecc..a515003 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -82,6 +82,7 @@ namespace Modelec if (!cap.isOpened()) { + RCLCPP_ERROR(get_logger(), "Failed to open camera at %s", link_.c_str()); error = "Failed to open camera"; return false; } @@ -95,6 +96,7 @@ namespace Modelec if (frame.empty()) { + RCLCPP_WARN(get_logger(), "Captured empty frame from camera"); error = "Empty frame"; return false; } @@ -115,6 +117,7 @@ namespace Modelec { std::string path = save_directory_ + generateImagePath(); cv::imwrite(path, frame); + RCLCPP_DEBUG(get_logger(), "Saved snapshot to %s", path.c_str()); } cap.release(); @@ -140,7 +143,6 @@ namespace Modelec response->message = join(colors, ";"); } - // 4 independent ROIs std::vector ColorDetector::classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const { std::vector results; @@ -151,6 +153,12 @@ namespace Modelec cv::Scalar mean = cv::mean(hsv(roi)); std::string color = classify(cv::Vec3d(mean[0], mean[1], mean[2])); + + RCLCPP_DEBUG(get_logger(), "ROI at (%d, %d, %d, %d) has mean HSV (%.2f, %.2f, %.2f) classified as %s", + roi.x, roi.y, roi.width, roi.height, + mean[0], mean[1], mean[2], + color.c_str()); + results.push_back(color); if (save_to_file_) @@ -172,22 +180,17 @@ namespace Modelec std::string ColorDetector::classify(const cv::Vec3d& hsv_roi) const { - cv::Scalar yellow_low(20, 100, 100), yellow_high(40, 255, 255); - cv::Scalar blue_low(100, 100, 100), blue_high(130, 255, 255); - - cv::Mat yellow_mask, blue_mask; - cv::inRange(hsv_roi, yellow_low, yellow_high, yellow_mask); - cv::inRange(hsv_roi, blue_low, blue_high, blue_mask); + double h = hsv_roi[0]; - int yellow_count = cv::countNonZero(yellow_mask); - int blue_count = cv::countNonZero(blue_mask); - int total_pixels = hsv_roi.rows * hsv_roi.cols; - - double yellow_ratio = static_cast(yellow_count) / total_pixels; - double blue_ratio = static_cast(blue_count) / total_pixels; + if (h >= 90 && h <= 130) + { + return "blue"; + } - if (yellow_ratio > blue_ratio && yellow_ratio > 0.3) return "yellow"; - if (blue_ratio > yellow_ratio && blue_ratio > 0.3) return "blue"; + if (h >= 20 && h <= 40) + { + return "yellow"; + } return "unknown"; } @@ -205,35 +208,19 @@ namespace Modelec void ColorDetector::SetupRois() { - rois_.resize(4); - - rois_[0] = { - Config::get("config.cam.points.first@x", 0), - Config::get("config.cam.points.first@y", 0), - Config::get("config.cam.points.first@w", 0), - Config::get("config.cam.points.first@h", 0) - }; - - rois_[1] = { - Config::get("config.cam.points.second@x", 0), - Config::get("config.cam.points.second@y", 0), - Config::get("config.cam.points.second@w", 0), - Config::get("config.cam.points.second@h", 0) - }; - - rois_[2] = { - Config::get("config.cam.points.third@x", 0), - Config::get("config.cam.points.third@y", 0), - Config::get("config.cam.points.third@w", 0), - Config::get("config.cam.points.third@h", 0) - }; - - rois_[3] = { - Config::get("config.cam.points.fourth@x", 0), - Config::get("config.cam.points.fourth@y", 0), - Config::get("config.cam.points.fourth@w", 0), - Config::get("config.cam.points.fourth@h", 0) - }; + auto count = Config::count("config.cam.rois.roi"); + + for (size_t i = 0; i < count; ++i) + { + std::string prefix = "config.cam.rois.roi[" + std::to_string(i) + "]"; + int x = Config::get(prefix + "@x", 0); + int y = Config::get(prefix + "@y", 0); + int width = Config::get(prefix + "@w", 100); + int height = Config::get(prefix + "@h", 100); + rois_.emplace_back(x, y, width, height); + + RCLCPP_DEBUG(get_logger(), "Loaded ROI %zu: x=%d, y=%d, width=%d, height=%d", i, x, y, width, height); + } } } diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index 3c4c1b5..affb543 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -81,11 +81,11 @@ http://192.168.1.21:8080/video true - - - - - - + + + + + + \ No newline at end of file diff --git a/src/modelec_utils/include/modelec_utils/config.hpp b/src/modelec_utils/include/modelec_utils/config.hpp index 8aba547..0d639ff 100644 --- a/src/modelec_utils/include/modelec_utils/config.hpp +++ b/src/modelec_utils/include/modelec_utils/config.hpp @@ -16,8 +16,12 @@ namespace Modelec template static T get(const std::string& key, const T& default_value = T()); + static size_t count(const std::string& prefix); + + static void printAll(); + private: - static void parseNode(tinyxml2::XMLElement* element, const std::string& prefix); + static void parseNode(tinyxml2::XMLElement* element, const std::string& key); static inline std::unordered_map values_; }; diff --git a/src/modelec_utils/src/config.cpp b/src/modelec_utils/src/config.cpp index 5044197..c566b52 100644 --- a/src/modelec_utils/src/config.cpp +++ b/src/modelec_utils/src/config.cpp @@ -1,3 +1,4 @@ +#include #include namespace Modelec @@ -15,27 +16,77 @@ namespace Modelec return false; } - parseNode(root, ""); + parseNode(root, root->Name()); return true; } - void Config::parseNode(tinyxml2::XMLElement* element, const std::string& prefix) { - std::string key_prefix = prefix.empty() ? element->Name() : prefix + "." + element->Name(); + size_t Config::count(const std::string& prefix) + { + size_t max_index = 0; + bool found = false; + + for (const auto& [key, _] : values_) + { + if (key.rfind(prefix + "[", 0) == 0) + { + auto start = key.find('[', prefix.size()); + auto end = key.find(']', start); + if (start != std::string::npos && end != std::string::npos) + { + size_t index = std::stoul(key.substr(start + 1, end - start - 1)); + max_index = std::max(max_index, index); + found = true; + } + } + } + + return found ? max_index + 1 : 0; + } + + void Config::printAll() + { + for (const auto& [key, value] : values_) + { + std::cout << key << " = " << value << std::endl; + } + } - const char* text = element->GetText(); - if (text && std::string(text).find_first_not_of(" \n\t") != std::string::npos) { - values_[key_prefix] = text; + void Config::parseNode(tinyxml2::XMLElement* element, const std::string& key) { + if (const char* text = element->GetText()) + { + if (std::string(text).find_first_not_of(" \n\t") != std::string::npos) + { + values_[key] = text; + } + } + + // Store attributes + for (auto* attr = element->FirstAttribute(); attr; attr = attr->Next()) + { + values_[key + "@" + attr->Name()] = attr->Value(); } - const tinyxml2::XMLAttribute* attr = element->FirstAttribute(); - while (attr) { - std::string attr_key = key_prefix + "@" + attr->Name(); - values_[attr_key] = attr->Value(); - attr = attr->Next(); + // Count children by name + std::unordered_map child_count; + for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) + { + child_count[child->Name()]++; } - for (tinyxml2::XMLElement* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) { - parseNode(child, key_prefix); + // Index children + std::unordered_map child_index; + + for (auto* child = element->FirstChildElement(); child; child = child->NextSiblingElement()) + { + std::string child_key = key + "." + child->Name(); + + if (child_count[child->Name()] > 1) + { + int index = child_index[child->Name()]++; + child_key += "[" + std::to_string(index) + "]"; + } + + parseNode(child, child_key); } } } \ No newline at end of file From d34ad74bf2e9cfbc95d7f41e2cca4b453cf350cb Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 11:26:17 +0100 Subject: [PATCH 75/81] box detector --- .../modelec_strat/missions/free_mission.hpp | 1 + .../include/modelec_strat/obstacle/box.hpp | 1 + src/modelec_strat/src/action_executor.cpp | 4 ++-- src/modelec_strat/src/missions/free_mission.cpp | 16 +++++++++++----- src/modelec_strat/src/obstacle/box.cpp | 15 +++++++++++++++ src/modelec_strat/src/strat_fms.cpp | 8 ++++---- 6 files changed, 34 insertions(+), 11 deletions(-) diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp index eab80ce..783eb3a 100644 --- a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -21,6 +21,7 @@ namespace Modelec { enum Step { GO_TO_FREE, + CHECK_BOX, DOWN, FREE_FIRST, ROTATE_ARM, diff --git a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp index 771822c..4cb70a2 100644 --- a/src/modelec_strat/include/modelec_strat/obstacle/box.hpp +++ b/src/modelec_strat/include/modelec_strat/obstacle/box.hpp @@ -33,6 +33,7 @@ namespace Modelec std::vector GetSide(Team team) const; void ParseColor(const std::string& colorStr); + void ParseColor(const std::vector& colorVec); protected: diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 3f36894..e97200c 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -221,10 +221,10 @@ namespace Modelec return; } - RCLCPP_INFO(node_->get_logger(), "Color detection succeeded: %s", res[1].c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Color detection succeeded: %s", msg->data.c_str()); if (box_obstacles_[cam_side_] != nullptr) { - box_obstacles_[cam_side_]->ParseColor(res[1]); + box_obstacles_[cam_side_]->ParseColor(std::vector(res.begin() + 1, res.end())); } else { RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); } diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp index 854c8d4..e53cbdf 100644 --- a/src/modelec_strat/src/missions/free_mission.cpp +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -24,6 +24,7 @@ namespace Modelec { std::swap(steps_, empty); steps_.push(GO_TO_FREE); + steps_.push(CHECK_BOX); steps_.push(DOWN); steps_.push(FREE_FIRST); steps_.push(ROTATE_ARM); @@ -37,7 +38,6 @@ namespace Modelec { { if (!action_executor_->IsActionDone()) { - // RCLCPP_INFO(node_->get_logger(), "FreeMission: Waiting for action to complete"); return; } @@ -100,10 +100,19 @@ namespace Modelec { } } + action_executor_->LookOn(BaseAction::Side::CENTER); + + go_timeout_ = node_->now(); + } + break; + case CHECK_BOX: + { auto obs = action_executor_->box_obstacles_[side_]; auto vect = obs->GetSide(nav_->GetTeamId() == NavigationHelper::BLUE ? BoxObstacle::BLUE : BoxObstacle::YELLOW); + RCLCPP_DEBUG(node_->get_logger(), "Box on side %d has %d boxes of his color side", static_cast(side_), static_cast(vect.size())); + if (vect.size() == 4) { std::queue empty; @@ -125,11 +134,8 @@ namespace Modelec { steps_.push(GO_BACK); steps_.push(DONE); } - - action_executor_->LookOn(BaseAction::Side::CENTER); - - go_timeout_ = node_->now(); } + break; case DOWN: { diff --git a/src/modelec_strat/src/obstacle/box.cpp b/src/modelec_strat/src/obstacle/box.cpp index bb3fe74..7e19e5f 100644 --- a/src/modelec_strat/src/obstacle/box.cpp +++ b/src/modelec_strat/src/obstacle/box.cpp @@ -110,4 +110,19 @@ namespace Modelec } } } + + void BoxObstacle::ParseColor(const std::vector& colorVec) + { + for (size_t i = 0; i < colorVec.size() && i < colors_.size(); ++i) + { + if (colorVec[i] == "yellow") + { + colors_[i] = YELLOW; + } + else if (colorVec[i] == "blue") + { + colors_[i] = BLUE; + } + } + } } diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 44c2381..ae93818 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -179,18 +179,18 @@ namespace Modelec case State::SELECT_MISSION: { - auto elapsed = now - match_start_time_; + auto duration = (now - match_start_time_).seconds(); - if (elapsed.seconds() >= 100) + if (duration >= 100) { Transition(State::STOP, "All missions done"); } - else if (elapsed.seconds() > 60 && !action_executor_->IsEmpty()) + else if (duration > 60 && !action_executor_->IsEmpty() && duration < 90) { Transition(State::FREE_MISSION, "No Time left, freeing boxes"); } - else if (elapsed.seconds() < 80) + else if (duration < 80) { Transition(State::SELECT_GAME_ACTION, "Selecting a game action"); } From 89f81539e0123f7a805ed51a22fc8ef13a297052 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 11:37:24 +0100 Subject: [PATCH 76/81] format --- src/modelec_com/src/color_detector.cpp | 4 ++-- src/modelec_strat/src/action_executor.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index a515003..b1c0c43 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -46,11 +46,11 @@ namespace Modelec if (!processSnapshot(colors, error)) { - res.data = "0;" + error; + res.data = "0|" + error; } else { - res.data = "1;" + join(colors, ";"); + res.data = "1|" + join(colors, ";"); } color_pub_->publish(res); diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index e97200c..c927d76 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -214,17 +214,17 @@ namespace Modelec color_sub_ = node_->create_subscription( "/action/detect_color/res", qos, [this](const std_msgs::msg::String::SharedPtr msg) { - auto res = split(msg->data, ';'); + auto res = split(msg->data, '|'); if (res.size() < 2 || res[0] != "1") { RCLCPP_WARN(node_->get_logger(), "Color detection failed: %s", msg->data.c_str()); return; } - RCLCPP_DEBUG(node_->get_logger(), "Color detection succeeded: %s", msg->data.c_str()); + RCLCPP_DEBUG(node_->get_logger(), "Color detection succeeded: %s", res[1].c_str()); if (box_obstacles_[cam_side_] != nullptr) { - box_obstacles_[cam_side_]->ParseColor(std::vector(res.begin() + 1, res.end())); + box_obstacles_[cam_side_]->ParseColor(res[1]); } else { RCLCPP_WARN(node_->get_logger(), "Color detection succeeded but no box in the cam side"); } From cdcaa791e1c4f91d4976accd2a0a36f1cf79b818 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 12:57:46 +0100 Subject: [PATCH 77/81] xml array support --- .../include/modelec_com/color_detector.hpp | 54 ++++++++++++++- src/modelec_com/src/color_detector.cpp | 35 +++------- src/modelec_strat/data/config.xml | 17 +++++ .../include/modelec_utils/config.hpp | 28 ++++++++ src/modelec_utils/test/config.test.cpp | 66 +++++++++++++++++++ src/modelec_utils/test/point.test.cpp | 4 +- 6 files changed, 174 insertions(+), 30 deletions(-) diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp index 53f4908..4f932b9 100644 --- a/src/modelec_com/include/modelec_com/color_detector.hpp +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -6,8 +6,17 @@ #include #include +#include + namespace Modelec { + struct ColorSetting + { + std::string name; + double h_min; + double h_max; + }; + class ColorDetector : public rclcpp::Node { public: @@ -27,8 +36,6 @@ namespace Modelec std::string generateImagePath() const; - void SetupRois(); - rclcpp::Service::SharedPtr service_; rclcpp::Subscription::SharedPtr ask_sub_; @@ -41,5 +48,48 @@ namespace Modelec std::string save_directory_ = "./"; bool enable_ = false; bool headless_ = true; + + std::vector color_configs_; }; + + template<> + inline std::vector + Config::get>( + const std::string& prefix, + const std::vector& default_value) + { + auto result = Config::getArray( + prefix, + [](const std::string& base) + { + return ColorSetting{ + Config::get(base + "@name"), + Config::get(base + "@hue_min"), + Config::get(base + "@hue_max") + }; + }); + + return result.empty() ? default_value : result; + } + + template<> + inline std::vector + Config::get>( + const std::string& prefix, + const std::vector& default_value) + { + auto result = Config::getArray( + prefix, + [](const std::string& base) + { + return cv::Rect( + Config::get(base + "@x", 0), + Config::get(base + "@y", 0), + Config::get(base + "@w", 100), + Config::get(base + "@h", 100) + ); + }); + + return result.empty() ? default_value : result; + } } diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index b1c0c43..7bfca23 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -1,6 +1,5 @@ #include #include -#include #include namespace Modelec @@ -58,7 +57,10 @@ namespace Modelec color_pub_ = create_publisher("action/detect_color/res", qos); - SetupRois(); + rois_ = Config::get>("config.cam.rois.roi", {}); + color_configs_ = Config::get>("config.cam.colors.color", {}); + + RCLCPP_DEBUG(get_logger(), "Loaded %zu ROIs and %zu color configs", rois_.size(), color_configs_.size()); if (!headless_) { @@ -182,14 +184,12 @@ namespace Modelec { double h = hsv_roi[0]; - if (h >= 90 && h <= 130) - { - return "blue"; - } - - if (h >= 20 && h <= 40) + for (const auto& color_config : color_configs_) { - return "yellow"; + if (h >= color_config.h_min && h <= color_config.h_max) + { + return color_config.name; + } } return "unknown"; @@ -205,23 +205,6 @@ namespace Modelec << ".png"; return ss.str(); } - - void ColorDetector::SetupRois() - { - auto count = Config::count("config.cam.rois.roi"); - - for (size_t i = 0; i < count; ++i) - { - std::string prefix = "config.cam.rois.roi[" + std::to_string(i) + "]"; - int x = Config::get(prefix + "@x", 0); - int y = Config::get(prefix + "@y", 0); - int width = Config::get(prefix + "@w", 100); - int height = Config::get(prefix + "@h", 100); - rois_.emplace_back(x, y, width, height); - - RCLCPP_DEBUG(get_logger(), "Loaded ROI %zu: x=%d, y=%d, width=%d, height=%d", i, x, y, width, height); - } - } } diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index affb543..b7dc812 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -87,5 +87,22 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/modelec_utils/include/modelec_utils/config.hpp b/src/modelec_utils/include/modelec_utils/config.hpp index 0d639ff..f391257 100644 --- a/src/modelec_utils/include/modelec_utils/config.hpp +++ b/src/modelec_utils/include/modelec_utils/config.hpp @@ -4,6 +4,8 @@ #include #include #include +#include +#include namespace Modelec { @@ -11,11 +13,19 @@ namespace Modelec { public: + + template + using BuilderFunc = std::function; + static bool load(const std::string& filepath); template static T get(const std::string& key, const T& default_value = T()); + template + static std::vector getArray(const std::string& prefix, + BuilderFunc builder = [](const std::string& base) { return get(base); }); + static size_t count(const std::string& prefix); static void printAll(); @@ -48,4 +58,22 @@ namespace Modelec auto str = get(key, default_value ? "true" : "false"); return str == "true" || str == "1"; } + + template + std::vector Config::getArray(const std::string& prefix, + BuilderFunc builder) + { + std::vector result; + + size_t n = Config::count(prefix); + result.reserve(n); + + for (size_t i = 0; i < n; ++i) + { + std::string base = prefix + "[" + std::to_string(i) + "]"; + result.emplace_back(builder(base)); + } + + return result; + } } diff --git a/src/modelec_utils/test/config.test.cpp b/src/modelec_utils/test/config.test.cpp index f856ca3..608eea3 100644 --- a/src/modelec_utils/test/config.test.cpp +++ b/src/modelec_utils/test/config.test.cpp @@ -48,3 +48,69 @@ TEST(ConfigTest, DefaultValues) EXPECT_EQ(Modelec::Config::get("missing.string", "default"), "default"); EXPECT_FALSE(Modelec::Config::get("missing.bool", false)); } + +TEST(ConfigTest, CountArray) +{ + // Create temporary XML file + const std::string filepath = "test_array_config.xml"; + + std::ofstream file(filepath); + file << + "" + " 1" + " 2" + " 3" + ""; + file.close(); + + EXPECT_TRUE(Modelec::Config::load(filepath)); + + EXPECT_EQ(Modelec::Config::count("root.item"), 3); +} + +TEST(ConfigTest, GetArray) +{ + // Create temporary XML file + const std::string filepath = "test_array_config.xml"; + + std::ofstream file(filepath); + file << + "" + " 1" + " 2" + " 3" + ""; + file.close(); + + struct Item + { + int value; + std::string attr; + }; + + EXPECT_TRUE(Modelec::Config::load(filepath)); + + auto array = Modelec::Config::getArray("root.item"); + + EXPECT_EQ(array.size(), 3); + EXPECT_EQ(array[0], 1); + EXPECT_EQ(array[1], 2); + EXPECT_EQ(array[2], 3); + + auto array2 = Modelec::Config::getArray("root.item", + [](const std::string& base) + { + return Item{ + Modelec::Config::get(base, 0), + Modelec::Config::get(base + "@a", "") + }; + }); + + EXPECT_EQ(array2.size(), 3); + EXPECT_EQ(array2[0].value, 1); + EXPECT_EQ(array2[0].attr, "b"); + EXPECT_EQ(array2[1].value, 2); + EXPECT_EQ(array2[1].attr, "c"); + EXPECT_EQ(array2[2].value, 3); + EXPECT_EQ(array2[2].attr, "d"); +} \ No newline at end of file diff --git a/src/modelec_utils/test/point.test.cpp b/src/modelec_utils/test/point.test.cpp index c57f394..6b7e79d 100644 --- a/src/modelec_utils/test/point.test.cpp +++ b/src/modelec_utils/test/point.test.cpp @@ -33,13 +33,13 @@ TEST(PointTest, GetTakePositionDefaultAngle) { TEST(PointTest, GetTakeBasePosition) { Modelec::Point p(0, 0, 0); Modelec::Point base = p.GetTakeBasePosition(); - EXPECT_EQ(base.x, 290); + EXPECT_EQ(base.x, BASE_DISTANCE); EXPECT_EQ(base.y, 0); } TEST(PointTest, GetTakeClosePosition) { Modelec::Point p(0, 0, 0); Modelec::Point close = p.GetTakeClosePosition(); - EXPECT_EQ(close.x, 165); + EXPECT_EQ(close.x, CLOSE_DISTANCE); EXPECT_EQ(close.y, 0); } From f92ec911c6ee6e7fd2ad7afda92894b2ebc03f07 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 13:08:00 +0100 Subject: [PATCH 78/81] color config --- src/modelec_strat/data/config.xml | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/modelec_strat/data/config.xml b/src/modelec_strat/data/config.xml index b7dc812..738bc91 100644 --- a/src/modelec_strat/data/config.xml +++ b/src/modelec_strat/data/config.xml @@ -88,21 +88,8 @@ - - - - - - - - - - - - - - - + + \ No newline at end of file From 12cbfba32819c3d4e8afa2e14691b3ecd6e30879 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 14:30:54 +0100 Subject: [PATCH 79/81] config to ros param --- .../include/modelec_com/color_detector.hpp | 43 ------- .../include/modelec_com/serial_listener.hpp | 5 +- src/modelec_com/src/color_detector.cpp | 58 +++++++--- src/modelec_com/src/pcb_action_interface.cpp | 11 +- src/modelec_com/src/pcb_odo_interface.cpp | 12 +- src/modelec_com/src/serial_listener.cpp | 7 +- src/modelec_core/config/config.yml | 109 ++++++++++++++++++ src/modelec_core/launch/modelec.launch.py | 34 +++--- src/modelec_gui/src/main.cpp | 7 -- src/modelec_gui/src/pages/home_page.cpp | 1 - src/modelec_gui/src/pages/map_page.cpp | 15 ++- src/modelec_gui/src/pages/test_map_page.cpp | 15 ++- .../include/modelec_strat/pami_manager.hpp | 6 - src/modelec_strat/src/enemy_manager.cpp | 41 +++---- .../src/missions/go_home_mission.cpp | 4 +- src/modelec_strat/src/navigation_helper.cpp | 107 +++++++++-------- src/modelec_strat/src/pami_manager.cpp | 30 ++--- src/modelec_strat/src/pathfinding.cpp | 44 ++++--- src/modelec_strat/src/strat_fms.cpp | 21 ++-- 19 files changed, 334 insertions(+), 236 deletions(-) create mode 100644 src/modelec_core/config/config.yml diff --git a/src/modelec_com/include/modelec_com/color_detector.hpp b/src/modelec_com/include/modelec_com/color_detector.hpp index 4f932b9..8c8c8fc 100644 --- a/src/modelec_com/include/modelec_com/color_detector.hpp +++ b/src/modelec_com/include/modelec_com/color_detector.hpp @@ -6,8 +6,6 @@ #include #include -#include - namespace Modelec { struct ColorSetting @@ -51,45 +49,4 @@ namespace Modelec std::vector color_configs_; }; - - template<> - inline std::vector - Config::get>( - const std::string& prefix, - const std::vector& default_value) - { - auto result = Config::getArray( - prefix, - [](const std::string& base) - { - return ColorSetting{ - Config::get(base + "@name"), - Config::get(base + "@hue_min"), - Config::get(base + "@hue_max") - }; - }); - - return result.empty() ? default_value : result; - } - - template<> - inline std::vector - Config::get>( - const std::string& prefix, - const std::vector& default_value) - { - auto result = Config::getArray( - prefix, - [](const std::string& base) - { - return cv::Rect( - Config::get(base + "@x", 0), - Config::get(base + "@y", 0), - Config::get(base + "@w", 100), - Config::get(base + "@h", 100) - ); - }); - - return result.empty() ? default_value : result; - } } diff --git a/src/modelec_com/include/modelec_com/serial_listener.hpp b/src/modelec_com/include/modelec_com/serial_listener.hpp index 5078f52..530e27b 100644 --- a/src/modelec_com/include/modelec_com/serial_listener.hpp +++ b/src/modelec_com/include/modelec_com/serial_listener.hpp @@ -30,7 +30,6 @@ namespace Modelec void start_async_write(); public: - std::string name_; boost::asio::serial_port port_; rclcpp::Publisher::SharedPtr publisher_; @@ -38,13 +37,13 @@ namespace Modelec SerialListener(); - SerialListener(const std::string& name, int bauds, const std::string& serial_port, + SerialListener(int bauds, const std::string& serial_port, int max_message_len); virtual ~SerialListener(); void close(); - void open(const std::string& name, int bauds, const std::string& serial_port, + void open(int bauds, const std::string& serial_port, int max_message_len); void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; } diff --git a/src/modelec_com/src/color_detector.cpp b/src/modelec_com/src/color_detector.cpp index 7bfca23..80615d8 100644 --- a/src/modelec_com/src/color_detector.cpp +++ b/src/modelec_com/src/color_detector.cpp @@ -5,19 +5,44 @@ namespace Modelec { ColorDetector::ColorDetector() - : Node("color_detector") + : Node("color_detector") { - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) + this->declare_parameter("enabled", true); + this->declare_parameter("link", ""); + this->declare_parameter("headless", true); + this->declare_parameter("save_to_file.enabled", true); + this->declare_parameter("save_to_file.path", "./"); + this->declare_parameter("rois", rclcpp::PARAMETER_INTEGER_ARRAY); + this->declare_parameter("colors.blue", rclcpp::PARAMETER_INTEGER_ARRAY); + this->declare_parameter("colors.yellow", rclcpp::PARAMETER_INTEGER_ARRAY); + + enable_ = this->get_parameter("enabled").as_bool(); + link_ = this->get_parameter("link").as_string(); + headless_ = this->get_parameter("headless").as_bool(); + save_to_file_ = this->get_parameter("save_to_file.enabled").as_bool(); + save_directory_ = this->get_parameter("save_to_file.path").as_string(); + + auto rois_param = this->get_parameter("rois").as_integer_array(); + for (size_t i = 0; i + 3 < rois_param.size(); i += 4) { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); + rois_.emplace_back(rois_param[i], rois_param[i + 1], rois_param[i + 2], rois_param[i + 3]); } - link_ = Config::get("config.cam.link", "/dev/video0"); - save_to_file_ = Config::get("config.cam.save_to_file.enabled", false); - save_directory_ = Config::get("config.cam.save_to_file.path", "./"); - enable_ = Config::get("config.cam.enabled", false); - headless_ = Config::get("config.cam.headless", true); + auto blue_param = this->get_parameter("colors.blue").as_integer_array(); + if (blue_param.size() >= 2) + { + color_configs_.push_back({ + "blue", static_cast(blue_param[0]), static_cast(blue_param[1]) + }); + } + + auto yellow_param = this->get_parameter("colors.yellow").as_integer_array(); + if (yellow_param.size() >= 2) + { + color_configs_.push_back({ + "yellow", static_cast(yellow_param[0]), static_cast(yellow_param[1]) + }); + } if (!enable_) { @@ -57,11 +82,6 @@ namespace Modelec color_pub_ = create_publisher("action/detect_color/res", qos); - rois_ = Config::get>("config.cam.rois.roi", {}); - color_configs_ = Config::get>("config.cam.colors.color", {}); - - RCLCPP_DEBUG(get_logger(), "Loaded %zu ROIs and %zu color configs", rois_.size(), color_configs_.size()); - if (!headless_) { cv::namedWindow("color_detector", cv::WINDOW_NORMAL); @@ -157,9 +177,9 @@ namespace Modelec std::string color = classify(cv::Vec3d(mean[0], mean[1], mean[2])); RCLCPP_DEBUG(get_logger(), "ROI at (%d, %d, %d, %d) has mean HSV (%.2f, %.2f, %.2f) classified as %s", - roi.x, roi.y, roi.width, roi.height, - mean[0], mean[1], mean[2], - color.c_str()); + roi.x, roi.y, roi.width, roi.height, + mean[0], mean[1], mean[2], + color.c_str()); results.push_back(color); @@ -201,8 +221,8 @@ namespace Modelec auto in_time_t = std::chrono::system_clock::to_time_t(now); std::stringstream ss; ss << "snapshot_" - << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S") - << ".png"; + << std::put_time(std::localtime(&in_time_t), "%Y%m%d_%H%M%S") + << ".png"; return ss.str(); } } diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index 3d1a5e2..7f6b17e 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -9,12 +9,11 @@ namespace Modelec // Service to create a new serial listener declare_parameter("serial_port", "/dev/USB_ACTION"); declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_action"); - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); + auto serial_port = get_parameter("serial_port").as_string(); + auto baudrate = get_parameter("baudrate").as_int(); + + RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate); asc_get_sub_ = this->create_subscription( "action/get/asc", 10, @@ -231,7 +230,7 @@ namespace Modelec }); - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); + this->open(baudrate, serial_port, MAX_MESSAGE_LEN); // TODO : check for real value there /*asc_value_mapper_ = { diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 6f2e9db..138a3c6 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -7,13 +7,11 @@ namespace Modelec { declare_parameter("serial_port", "/dev/USB_ODO"); declare_parameter("baudrate", 115200); - declare_parameter("name", "pcb_odo"); - // Service to create a new serial listener - auto request = std::make_shared(); - request->name = get_parameter("name").as_string(); - request->bauds = get_parameter("baudrate").as_int(); - request->serial_port = get_parameter("serial_port").as_string(); + auto serial_port = get_parameter("serial_port").as_string(); + auto baudrate = get_parameter("baudrate").as_int(); + + RCLCPP_INFO(this->get_logger(), "Starting PCB Odometry Interface on port %s with baudrate %ld", serial_port.c_str(), baudrate); odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); @@ -104,7 +102,7 @@ namespace Modelec SendOrder("START", {std::to_string(msg->data)}); }); - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); + this->open(baudrate, serial_port, MAX_MESSAGE_LEN); SetPID("THETA", 14, 0, 0); SetPID("POS", 5, 0, 0); diff --git a/src/modelec_com/src/serial_listener.cpp b/src/modelec_com/src/serial_listener.cpp index 29a8c6e..cb78962 100644 --- a/src/modelec_com/src/serial_listener.cpp +++ b/src/modelec_com/src/serial_listener.cpp @@ -7,10 +7,10 @@ namespace Modelec { } - SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port, + SerialListener::SerialListener(int bauds, const std::string& serial_port, int max_message_len) : io_(), port_(io_) { - open(name, bauds, serial_port, max_message_len); + open(bauds, serial_port, max_message_len); } SerialListener::~SerialListener() @@ -32,9 +32,8 @@ namespace Modelec } } - void SerialListener::open(const std::string& name, int bauds, const std::string& serial_port, + void SerialListener::open(int bauds, const std::string& serial_port, int max_message_len) { - this->name_ = name; this->bauds_ = bauds; this->serial_port_ = serial_port; this->max_message_len_ = max_message_len; diff --git a/src/modelec_core/config/config.yml b/src/modelec_core/config/config.yml new file mode 100644 index 0000000..270f8fb --- /dev/null +++ b/src/modelec_core/config/config.yml @@ -0,0 +1,109 @@ +enemy_manager: + ros__parameters: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 50 + detection: + min_move_threshold_mm: 50 + refresh_rate: 3 + max_stationary_time_s: 0.5 + min_emergency_distance_mm: 300 + margin_detection_table_mm: 50 + factor_close_enemy: -2 + +pami_manager: + ros__parameters: + time_to_put_zone: 77 + time_to_remove_top_pot: 65 + +strat_fsm: + ros__parameters: + robot: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 100 + + map: + size: + grid_width: 600 + grid_height: 400 + map_width_mm: 3000 + map_height_mm: 2000 + + spawn: + yellow_top: + x: 350 + y: 1700 + theta: -1.5708 + blue_top: + x: 2650 + y: 1700 + theta: -1.5708 + width_mm: 450 + height_mm: 450 + + home: + blue: + x: 2650 + y: 1700 + theta: -1.5708 + yellow: + x: 350 + y: 1700 + theta: -1.5708 + + static_strat: false + factor: + theta: 20 + obs: 0.6 + thermo: 0.7 + timer_period_ms: 20 + + thermo: + yellow: + start: + x: 200 + y: 175 + theta: 0 + finish: + x: 700 + y: 175 + theta: 0 + blue: + start: + x: 2800 + y: 175 + theta: 3.14 + finish: + x: 2300 + y: 175 + theta: 3.14 + + mission_score: + go_home: 10 + +color_detector: + ros__parameters: + save_to_file: + enabled: true + path: "./cam/" + enabled: true + # link: "/dev/video0" + link: "http://192.168.1.21:8080/video" + headless: true + rois: [300, 700, 50, 50, 700, 700, 50, 50, 1200, 700, 50, 50, 1500, 700, 50, 50] + colors: + blue: [90, 130] + yellow: [20, 40] + +pcb_odo_manager: + ros__parameters: + serial_port: '/dev/USB_ODO' + baudrate: 115200 + +pcb_action_manager: + ros__parameters: + serial_port: '/dev/USB_ACTION' + baudrate: 115200 diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 7d55ca7..9b1dfe2 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -1,3 +1,4 @@ +import os from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -10,9 +11,10 @@ from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node - +from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + pkg_name = 'modelec_core' # ------------------------------------------------- # Launch arguments # ------------------------------------------------- @@ -48,6 +50,12 @@ def generate_launch_description(): angle_compensate = LaunchConfiguration('angle_compensate', default='false') scan_mode = LaunchConfiguration('scan_mode', default='Standard') + param_file = os.path.join( + get_package_share_directory(pkg_name), + 'config', + 'config.yaml' + ) + # ------------------------------------------------- # RPLIDAR with auto-restart # ------------------------------------------------- @@ -56,7 +64,7 @@ def create_lidar_with_restart(): package='rplidar_ros', executable='rplidar_node', name='rplidar_node', - parameters=[{ + parameters=[param_file, { 'channel_type': channel_type, 'serial_port': serial_port, 'serial_baudrate': serial_baudrate, @@ -65,7 +73,6 @@ def create_lidar_with_restart(): 'angle_compensate': angle_compensate, 'scan_mode': scan_mode, }], - output='screen', ) restart_handler = RegisterEventHandler( @@ -106,6 +113,7 @@ def launch_gui(context, *args, **kwargs): package='modelec_gui', executable='modelec_gui', name='modelec_gui', + parameters=[param_file], ) shutdown = RegisterEventHandler( @@ -127,21 +135,13 @@ def launch_com(context, *args, **kwargs): package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface', - parameters=[{ - 'serial_port': '/dev/USB_ODO', - 'baudrate': 115200, - 'name': 'pcb_odo', - }], + parameters=[param_file], ), Node( package='modelec_com', executable='pcb_action_interface', name='pcb_action_interface', - parameters=[{ - 'serial_port': '/dev/USB_ACTION', - 'baudrate': 115200, - 'name': 'pcb_action', - }], + parameters=[param_file], ), ] return [] @@ -156,13 +156,14 @@ def launch_strat(context, *args, **kwargs): package='modelec_strat', executable='strat_fsm', name='strat_fsm', + parameters=[param_file], # prefix=['xterm -e gdb -ex run --args'], - # output='screen', ), Node( package='modelec_strat', executable='pami_manager', name='pami_manager', + parameters=[param_file], ), ] return [] @@ -177,6 +178,7 @@ def launch_enemy_manager(context, *args, **kwargs): package='modelec_strat', executable='enemy_manager', name='enemy_manager', + parameters=[param_file], ) ] return [] @@ -191,7 +193,7 @@ def launch_joy(context, *args, **kwargs): package='joy', executable='joy_node', name='joy_node', - output='screen', + parameters=[param_file], ) ] return [] @@ -203,7 +205,7 @@ def launch_color_detector(context, *args, **kwargs): package='modelec_com', executable='color_detector', name='color_detector', - output='screen', + parameters=[param_file], ) ] return [] diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index 2e45856..2b8c4c6 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -4,7 +4,6 @@ #include "modelec_gui/modelec_gui.hpp" #include -#include int main(int argc, char **argv) { @@ -12,12 +11,6 @@ int main(int argc, char **argv) rclcpp::init(argc, argv); - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Modelec::Config::load(config_path)) - { - RCLCPP_ERROR(rclcpp::get_logger("modelec_gui"), "Failed to load configuration file: %s", config_path.c_str()); - } - auto node = rclcpp::Node::make_shared("qt_gui_node"); ModelecGUI::ROS2QtGUI window(node); diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 5bc00ac..c9fbfb2 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -2,7 +2,6 @@ #include #include -#include namespace ModelecGUI { diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 2d0c1ee..e2713cc 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -3,7 +3,6 @@ #include #include -#include #include namespace ModelecGUI @@ -49,11 +48,17 @@ namespace ModelecGUI qpoints = {}; - robot_length_ = Modelec::Config::get("config.robot.size.length_mm", 200); - robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 300); + node_->declare_parameter("map.size.map_width_mm", 3000); + node_->declare_parameter("map.size.map_height_mm", 2000); - enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); - enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); + node_->declare_parameter("robot.size.length_mm", 200); + node_->declare_parameter("robot.size.width_mm", 300); + + robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); + robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); + + enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int(); + enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int(); add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index fc17e98..0633fa0 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -3,7 +3,6 @@ #include #include -#include #include namespace ModelecGUI @@ -23,11 +22,17 @@ namespace ModelecGUI qpoints = {}; - robot_length_ = Modelec::Config::get("config.robot.size.length_mm", 200); - robot_width_ = Modelec::Config::get("config.robot.size.width_mm", 300); + node_->declare_parameter("map.size.map_width_mm", 3000); + node_->declare_parameter("map.size.map_height_mm", 2000); - enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); - enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); + node_->declare_parameter("robot.size.length_mm", 200); + node_->declare_parameter("robot.size.width_mm", 300); + + robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); + robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); + + enemy_length_ = node_->get_parameter("enemy.size.length_mm").as_int(); + enemy_width_ = node_->get_parameter("enemy.size.width_mm").as_int(); add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, diff --git a/src/modelec_strat/include/modelec_strat/pami_manager.hpp b/src/modelec_strat/include/modelec_strat/pami_manager.hpp index 2dee336..c46aa34 100644 --- a/src/modelec_strat/include/modelec_strat/pami_manager.hpp +++ b/src/modelec_strat/include/modelec_strat/pami_manager.hpp @@ -2,7 +2,6 @@ #include -#include #include #include @@ -29,11 +28,6 @@ namespace Modelec int score_to_add_ = 0; - int score_goupie_ = 0; - int score_superstar_ = 0; - int score_all_party_ = 0; - int score_free_zone_ = 0; - rclcpp::TimerBase::SharedPtr timer_add_; // rclcpp::TimerBase::SharedPtr timer_remove_; diff --git a/src/modelec_strat/src/enemy_manager.cpp b/src/modelec_strat/src/enemy_manager.cpp index 750149d..8f325c4 100644 --- a/src/modelec_strat/src/enemy_manager.cpp +++ b/src/modelec_strat/src/enemy_manager.cpp @@ -7,28 +7,29 @@ namespace Modelec { EnemyManager::EnemyManager() : Node("enemy_manager") { - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } - - min_move_threshold_mm_ = Config::get("config.enemy.detection.min_move_threshold_mm", 50); - - refresh_rate_s_ = Config::get("config.enemy.detection.refresh_rate", 1.0); + this->declare_parameter("enemy.detection.min_move_threshold_mm", 50.0); + this->declare_parameter("enemy.detection.refresh_rate", 1.0); + this->declare_parameter("enemy.detection.max_stationary_time_s", 10.0); + this->declare_parameter("enemy.detection.margin_detection_table_mm", 50.0); + this->declare_parameter("enemy.detection.min_emergency_distance_mm", 500.0); - max_stationary_time_s_ = Config::get("config.enemy.detection.max_stationary_time_s", 10.0); + this->declare_parameter("map.size.map_width_mm", 3000.0); + this->declare_parameter("map.size.map_height_mm", 2000.0); - map_width_ = Config::get("config.map.size.map_width_mm", 3000.0); - map_height_ = Config::get("config.map.size.map_height_mm", 2000.0); + this->declare_parameter("robot.size.width_mm", 500.0); + this->declare_parameter("robot.size.length_mm", 500.0); - margin_detection_table_ = Config::get("config.enemy.detection.margin_detection_table_mm", 50.0); + min_move_threshold_mm_ = this->get_parameter("enemy.detection.min_move_threshold_mm").as_double(); + refresh_rate_s_ = this->get_parameter("enemy.detection.refresh_rate").as_double(); + max_stationary_time_s_ = this->get_parameter("enemy.detection.max_stationary_time_s").as_double(); + margin_detection_table_ = this->get_parameter("enemy.detection.margin_detection_table_mm").as_double(); + min_emergency_distance_ = this->get_parameter("enemy.detection.min_emergency_distance_mm").as_double(); - robot_width_ = Config::get("config.robot.size.width_mm", 500.0); - robot_length_ = Config::get("config.robot.size.length_mm", 500.0); - robot_radius_ = std::max(robot_width_, robot_length_) * 0.4; + map_width_ = this->get_parameter("map.size.map_width_mm").as_double(); + map_height_ = this->get_parameter("map.size.map_height_mm").as_double(); - min_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 500.0f); + robot_width_ = this->get_parameter("robot.size.width_mm").as_double(); + robot_length_ = this->get_parameter("robot.size.length_mm").as_double(); current_pos_sub_ = this->create_subscription( "odometry/position", 10, @@ -50,7 +51,7 @@ namespace Modelec close_enemy_pos_pub_ = this->create_publisher( "enemy/position/emergency", 10); - state_sub_ = create_subscription("/strat/state", 10, + state_sub_ = create_subscription("strat/state", 10, [this]( const modelec_interfaces::msg::StratState::SharedPtr @@ -69,7 +70,7 @@ namespace Modelec }); enemy_long_time_pub_ = this->create_publisher( - "/enemy/long_time", 10); + "enemy/long_time", 10); start_sub_ = this->create_subscription( "lidar/start", 10, @@ -80,7 +81,7 @@ namespace Modelec }); timer_ = this->create_wall_timer( - std::chrono::seconds(static_cast(refresh_rate_s_)), + std::chrono::milliseconds(static_cast(refresh_rate_s_ * 1000.0)), [this]() { TimerCallback(); diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 8e8026e..9b11d74 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -13,7 +13,9 @@ namespace Modelec { node_ = node; - mission_score_ = Config::get("config.mission_score.go_home", 0); + node->declare_parameter("mission_score.go_home", 0); + + mission_score_ = node->get_parameter("mission_score.go_home").as_int(); score_pub_ = node_->create_publisher("/strat/score", 10); diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index b0e7ac1..cb54df1 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -1,7 +1,6 @@ #include #include #include -#include namespace Modelec { @@ -10,12 +9,43 @@ namespace Modelec NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node) { - pathfinding_ = std::make_shared(node); - - factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); - factor_theta_ = Config::get("config.factor.theta", 20.f); + node_->declare_parameter("factor_close_enemy", -0.5); + node_->declare_parameter("factor.theta", 20.0); + node_->declare_parameter("enemy.detection.min_emergency_distance_mm", 390); + + factor_close_enemy_ = node_->get_parameter("factor_close_enemy").as_double(); + factor_theta_ = node_->get_parameter("factor.theta").as_double(); + enemy_emergency_distance_ = node_->get_parameter("enemy.detection.min_emergency_distance_mm").as_int(); + + node_->declare_parameter("home.yellow.x", 0); + node_->declare_parameter("home.yellow.y", 0); + node_->declare_parameter("home.yellow.theta", 0.0); + node_->declare_parameter("home.blue.x", 0); + node_->declare_parameter("home.blue.y", 0); + node_->declare_parameter("home.blue.theta", 0.0); + + node_->declare_parameter("thermo.yellow.start.x", 0); + node_->declare_parameter("thermo.yellow.start.y", 0); + node_->declare_parameter("thermo.yellow.start.theta", 0.0); + node_->declare_parameter("thermo.yellow.finish.x", 0); + node_->declare_parameter("thermo.yellow.finish.y", 0); + node_->declare_parameter("thermo.yellow.finish.theta", 0.0); + + node_->declare_parameter("thermo.blue.start.x", 0); + node_->declare_parameter("thermo.blue.start.y", 0); + node_->declare_parameter("thermo.blue.start.theta", 0.0); + node_->declare_parameter("thermo.blue.finish.x", 0); + node_->declare_parameter("thermo.blue.finish.y", 0); + node_->declare_parameter("thermo.blue.finish.theta", 0.0); + + node_->declare_parameter("spawn.yellow_top.x", 0); + node_->declare_parameter("spawn.yellow_top.y", 0); + node_->declare_parameter("spawn.yellow_top.theta", 0.0); + node_->declare_parameter("spawn.blue_top.x", 0); + node_->declare_parameter("spawn.blue_top.y", 0); + node_->declare_parameter("spawn.blue_top.theta", 0.0); - enemy_emergency_distance_ = Config::get("config.enemy.detection.min_emergency_distance_mm", 390); + pathfinding_ = std::make_shared(node); SetupSpawn(); @@ -536,46 +566,33 @@ namespace Modelec PosMsg::SharedPtr NavigationHelper::GetHomePosition() { PosMsg::SharedPtr home = std::make_shared(); - if (team_id_ == YELLOW) - { - home->x = Config::get("config.home.yellow@x", 0); - home->y = Config::get("config.home.yellow@y", 0); - home->theta = Config::get("config.home.yellow@theta", 0); - } - else - { - home->x = Config::get("config.home.blue@x", 0); - home->y = Config::get("config.home.blue@y", 0); - home->theta = Config::get("config.home.blue@theta", 0); - } + + std::string prefix = (team_id_ == YELLOW) ? "home.yellow" : "home.blue"; + + home->x = node_->get_parameter(prefix + ".x").as_int(); + home->y = node_->get_parameter(prefix + ".y").as_int(); + home->theta = node_->get_parameter(prefix + ".theta").as_double(); + return home; } std::array NavigationHelper::GetThermoPositions() { - Point thermoPosStart = Point(); - Point thermoPosFinish = Point(); - if (team_id_ == YELLOW) - { - thermoPosStart.x = Config::get("config.thermo.yellow.start@x", 0); - thermoPosStart.y = Config::get("config.thermo.yellow.start@y", 0); - thermoPosStart.theta = Config::get("config.thermo.yellow.start@theta", 0); + std::string prefix = (team_id_ == YELLOW) ? "thermo.yellow" : "thermo.blue"; - thermoPosFinish.x = Config::get("config.thermo.yellow.finish@x", 0); - thermoPosFinish.y = Config::get("config.thermo.yellow.finish@y", 0); - thermoPosFinish.theta = Config::get("config.thermo.yellow.finish@theta", 0); - } - else - { - thermoPosStart.x = Config::get("config.thermo.blue.start@x", 0); - thermoPosStart.y = Config::get("config.thermo.blue.start@y", 0); - thermoPosStart.theta = Config::get("config.thermo.blue.start@theta", 0); + Point start( + node_->get_parameter(prefix + ".start.x").as_int(), + node_->get_parameter(prefix + ".start.y").as_int(), + node_->get_parameter(prefix + ".start.theta").as_double() + ); - thermoPosFinish.x = Config::get("config.thermo.blue.finish@x", 0); - thermoPosFinish.y = Config::get("config.thermo.blue.finish@y", 0); - thermoPosFinish.theta = Config::get("config.thermo.blue.finish@theta", 0); - } - return {thermoPosStart, thermoPosFinish}; + Point finish( + node_->get_parameter(prefix + ".finish.x").as_int(), + node_->get_parameter(prefix + ".finish.y").as_int(), + node_->get_parameter(prefix + ".finish.theta").as_double() + ); + + return {start, finish}; } void NavigationHelper::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) @@ -787,15 +804,15 @@ namespace Modelec void NavigationHelper::SetupSpawn() { spawn_yellow_["top"] = Point( - Config::get("config.spawn.yellow.top@x"), - Config::get("config.spawn.yellow.top@y"), - Config::get("config.spawn.yellow.top@theta") + node_->get_parameter("spawn.yellow_top.x").as_int(), + node_->get_parameter("spawn.yellow_top.y").as_int(), + node_->get_parameter("spawn.yellow_top.theta").as_double() ); spawn_blue_["top"] = Point( - Config::get("config.spawn.blue.top@x"), - Config::get("config.spawn.blue.top@y"), - Config::get("config.spawn.blue.top@theta") + node_->get_parameter("spawn.blue_top.x").as_int(), + node_->get_parameter("spawn.blue_top.y").as_int(), + node_->get_parameter("spawn.blue_top.theta").as_double() ); } } \ No newline at end of file diff --git a/src/modelec_strat/src/pami_manager.cpp b/src/modelec_strat/src/pami_manager.cpp index 7b13442..052a567 100644 --- a/src/modelec_strat/src/pami_manager.cpp +++ b/src/modelec_strat/src/pami_manager.cpp @@ -5,23 +5,13 @@ namespace Modelec { PamiManger::PamiManger() : Node("pami_manager") { - std::string config_file = ament_index_cpp::get_package_share_directory("modelec_strat") + - "/data/config.xml"; + this->declare_parameter("time_to_put_zone", 80); + this->declare_parameter("time_to_remove_top_pot", 70); - if (!Config::load(config_file)) - { - RCLCPP_ERROR(get_logger(), "Failed to load configuration"); - } - - time_to_put_zone_ = Config::get("config.pami.time_to_put_zone", 80); - time_to_remove_top_pot_ = Config::get("config.pami.time_to_remove_top_pot", 70); + time_to_put_zone_ = this->get_parameter("time_to_put_zone").as_int(); + time_to_remove_top_pot_ = this->get_parameter("time_to_remove_top_pot").as_int(); - score_goupie_ = Config::get("config.mission_score.pami.goupie"); - score_superstar_ = Config::get("config.mission_score.pami.superstar"); - score_all_party_ = Config::get("config.mission_score.pami.all_party"); - score_free_zone_ = 0; - - score_to_add_ = 0; //score_goupie_ + score_superstar_ + /*score_all_party_ +*/ score_free_zone_; + score_to_add_ = 0; std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/pami_zone.xml"; @@ -31,9 +21,9 @@ namespace Modelec } start_time_sub_ = create_subscription( - "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr msg) + "/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr) { - auto now = std::chrono::system_clock::now().time_since_epoch(); + /*auto now = std::chrono::system_clock::now().time_since_epoch(); auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_); auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_); @@ -51,7 +41,7 @@ namespace Modelec } timer_add_->cancel(); - }); + });*/ }); add_obs_pub_ = create_publisher( @@ -65,10 +55,10 @@ namespace Modelec reset_strat_sub_ = create_subscription( "/strat/reset", 10, [this](const std_msgs::msg::Empty::SharedPtr) { - if (timer_add_) + /*if (timer_add_) { timer_add_->cancel(); - } + }*/ /*if (timer_remove_) { timer_remove_->cancel(); diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index c6d4bf2..9775ca8 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -1,6 +1,5 @@ #include #include -#include namespace Modelec { struct AStarNode { @@ -23,22 +22,33 @@ namespace Modelec { } Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr &node) : node_(node) { - map_width_mm_ = Config::get("config.map.size.map_width_mm", 3000); - map_height_mm_ = Config::get("config.map.size.map_height_mm", 2000); - - robot_length_mm_ = Config::get("config.robot.size.length_mm", 300); - robot_width_mm_ = Config::get("config.robot.size.width_mm", 300); - margin_mm_ = Config::get("config.robot.size.margin_mm", 100); - - enemy_length_mm_ = Config::get("config.enemy.size.length_mm", 300); - enemy_width_mm_ = Config::get("config.enemy.size.width_mm", 300); - - enemy_margin_mm_ = Config::get("config.enemy.size.margin_mm", 50); - - factor_close_enemy_ = Config::get("config.enemy.factor_close_enemy", -0.5f); - - grid_width_ = Config::get("config.map.size.grid_width", 300); - grid_height_ = Config::get("config.map.size.grid_height", 200); + node_->declare_parameter("map.size.map_width_mm", 3000); + node_->declare_parameter("map.size.map_height_mm", 2000); + node_->declare_parameter("map.size.grid_width", 300); + node_->declare_parameter("map.size.grid_height", 200); + + map_width_mm_ = node_->get_parameter("map.size.map_width_mm").as_int(); + map_height_mm_ = node_->get_parameter("map.size.map_height_mm").as_int(); + grid_width_ = node_->get_parameter("map.size.grid_width").as_int(); + grid_height_ = node_->get_parameter("map.size.grid_height").as_int(); + + node_->declare_parameter("robot.size.length_mm", 300); + node_->declare_parameter("robot.size.width_mm", 300); + node_->declare_parameter("robot.size.margin_mm", 100); + + robot_length_mm_ = node_->get_parameter("robot.size.length_mm").as_int(); + robot_width_mm_ = node_->get_parameter("robot.size.width_mm").as_int(); + margin_mm_ = node_->get_parameter("robot.size.margin_mm").as_int(); + + node_->declare_parameter("enemy.size.length_mm", 300); + node_->declare_parameter("enemy.size.width_mm", 300); + node_->declare_parameter("enemy.size.margin_mm", 50); + node_->declare_parameter("enemy.factor_close_enemy", -0.5); + + enemy_length_mm_ = node_->get_parameter("enemy.size.length_mm").as_int(); + enemy_width_mm_ = node_->get_parameter("enemy.size.width_mm").as_int(); + enemy_margin_mm_ = node_->get_parameter("enemy.size.margin_mm").as_int(); + factor_close_enemy_ = node_->get_parameter("enemy.factor_close_enemy").as_double(); std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/obstacles.xml"; diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index ae93818..6f85f5c 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -15,6 +15,16 @@ namespace Modelec StratFMS::StratFMS() : Node("start_fms") { + this->declare_parameter("static_strat", false); + this->declare_parameter("factor.obs", 1.0); + this->declare_parameter("factor.thermo", 0.5); + this->declare_parameter("timer_period_ms", 100); + + static_strat_ = this->get_parameter("static_strat").as_bool(); + factor_obs_ = this->get_parameter("factor.obs").as_double(); + factor_thermo_ = this->get_parameter("factor.thermo").as_double(); + timer_period_ms_ = this->get_parameter("timer_period_ms").as_int(); + tir_sub_ = create_subscription( "/action/tir/start", 10, [this](const std_msgs::msg::Empty::SharedPtr) { @@ -59,17 +69,6 @@ namespace Modelec "/action/tir/arm/set", 10); start_odo_pub_ = create_publisher("/odometry/start", 10); - - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } - - static_strat_ = Config::get("config.static_strat", false); - factor_obs_ = Config::get("config.factor.obs", 1.0); - factor_thermo_ = Config::get("config.factor.thermo", 0.5); - timer_period_ms_ = Config::get("config.timer_period_ms", 100); } void StratFMS::Init() From 5c68957f5b026826fcd293ac7afbc13ea83182e9 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 15:49:30 +0100 Subject: [PATCH 80/81] config to ros param --- src/modelec_core/config/config.yml | 35 ++++++++++++++++++--- src/modelec_core/launch/modelec.launch.py | 2 +- src/modelec_gui/src/main.cpp | 9 ++++++ src/modelec_gui/src/pages/map_page.cpp | 7 ++--- src/modelec_gui/src/pages/test_map_page.cpp | 7 ++--- 5 files changed, 44 insertions(+), 16 deletions(-) diff --git a/src/modelec_core/config/config.yml b/src/modelec_core/config/config.yml index 270f8fb..c4d6295 100644 --- a/src/modelec_core/config/config.yml +++ b/src/modelec_core/config/config.yml @@ -17,6 +17,25 @@ pami_manager: time_to_put_zone: 77 time_to_remove_top_pot: 65 +modelec_gui: + ros__parameters: + robot: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 100 + map: + size: + grid_width: 600 + grid_height: 400 + map_width_mm: 3000 + map_height_mm: 2000 + enemy: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 50 + strat_fsm: ros__parameters: robot: @@ -25,6 +44,12 @@ strat_fsm: length_mm: 300 margin_mm: 100 + enemy: + size: + width_mm: 300 + length_mm: 300 + margin_mm: 50 + map: size: grid_width: 600 @@ -56,7 +81,7 @@ strat_fsm: static_strat: false factor: - theta: 20 + theta: 20.0 obs: 0.6 thermo: 0.7 timer_period_ms: 20 @@ -66,11 +91,11 @@ strat_fsm: start: x: 200 y: 175 - theta: 0 + theta: 0.0 finish: x: 700 y: 175 - theta: 0 + theta: 0.0 blue: start: x: 2800 @@ -98,12 +123,12 @@ color_detector: blue: [90, 130] yellow: [20, 40] -pcb_odo_manager: +pcb_odo_interface: ros__parameters: serial_port: '/dev/USB_ODO' baudrate: 115200 -pcb_action_manager: +pcb_action_interface: ros__parameters: serial_port: '/dev/USB_ACTION' baudrate: 115200 diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 9b1dfe2..72c9fb1 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): param_file = os.path.join( get_package_share_directory(pkg_name), 'config', - 'config.yaml' + 'config.yml' ) # ------------------------------------------------- diff --git a/src/modelec_gui/src/main.cpp b/src/modelec_gui/src/main.cpp index 2b8c4c6..6015fdb 100644 --- a/src/modelec_gui/src/main.cpp +++ b/src/modelec_gui/src/main.cpp @@ -13,6 +13,15 @@ int main(int argc, char **argv) auto node = rclcpp::Node::make_shared("qt_gui_node"); + node->declare_parameter("map.size.map_width_mm", 3000); + node->declare_parameter("map.size.map_height_mm", 2000); + + node->declare_parameter("robot.size.length_mm", 200); + node->declare_parameter("robot.size.width_mm", 300); + + node->declare_parameter("enemy.size.length_mm", 200); + node->declare_parameter("enemy.size.width_mm", 300); + ModelecGUI::ROS2QtGUI window(node); window.show(); diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index e2713cc..7ff6f4c 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -48,11 +48,8 @@ namespace ModelecGUI qpoints = {}; - node_->declare_parameter("map.size.map_width_mm", 3000); - node_->declare_parameter("map.size.map_height_mm", 2000); - - node_->declare_parameter("robot.size.length_mm", 200); - node_->declare_parameter("robot.size.width_mm", 300); + map_height_ = node_->get_parameter("map.size.map_height_mm").as_int(); + map_width_ = node_->get_parameter("map.size.map_width_mm").as_int(); robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index 0633fa0..a31af6b 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -22,11 +22,8 @@ namespace ModelecGUI qpoints = {}; - node_->declare_parameter("map.size.map_width_mm", 3000); - node_->declare_parameter("map.size.map_height_mm", 2000); - - node_->declare_parameter("robot.size.length_mm", 200); - node_->declare_parameter("robot.size.width_mm", 300); + map_height_ = node_->get_parameter("map.size.map_height_mm").as_int(); + map_width_ = node_->get_parameter("map.size.map_width_mm").as_int(); robot_length_ = node_->get_parameter("robot.size.length_mm").as_int(); robot_width_ = node_->get_parameter("robot.size.width_mm").as_int(); From 821b42befeaf17394799eddf69aeff703253bb93 Mon Sep 17 00:00:00 2001 From: acki Date: Sat, 7 Feb 2026 15:52:26 +0100 Subject: [PATCH 81/81] gitignore --- .gitignore | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index aabb679..468f66c 100644 --- a/.gitignore +++ b/.gitignore @@ -106,4 +106,5 @@ AMENT_IGNORE .vscode .cache -.idea \ No newline at end of file +.idea +cam \ No newline at end of file