diff --git a/build.zsh b/build.zsh index f783a7b..fd96718 100755 --- a/build.zsh +++ b/build.zsh @@ -1,3 +1,3 @@ -colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DCMAKE_BUILD_TYPE=Debug source "install/setup.zsh" diff --git a/fastdds_setup.xml b/fastdds_setup.xml index 44bf467..de973c3 100644 --- a/fastdds_setup.xml +++ b/fastdds_setup.xml @@ -23,12 +23,12 @@ -
acki-windows.tail1f0d2.ts.net
+
100.113.219.4
 -
modelec-robot.ayu-hops.ts.net
+
100.73.69.106
 diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py index 29bad6d..cab53ea 100644 --- a/simulated_pcb/action.py +++ b/simulated_pcb/action.py @@ -1,34 +1,47 @@ import argparse - import serial -import time -import math import threading +import time class SimulatedPCB: - def __init__(self, port='/dev/pts/6', baud=115200): - self.ser = serial.Serial(port, baud, timeout=1) + def __init__(self, port='/tmp/ACTION_SIM', baud=115200): + self.ser = serial.Serial(port, baud, timeout=0.1) self.running = True - self.last_update = time.time() - # État simulé - self.servos = {0: 0, 1: 0, 2: 0, 3: 0, 4: 0, 5: 0, } # {id: pos_index} - self.servo_angles = {0: {0: 0, 1: 0, 3: 0}, 1: {0: 0, 1: 0, 3: 0}, 2: {0: 0, 1: 0, 3: 0}, 3: {0: 0, 1: 0, 3: 0}, 4: {0: 0, 1: 0, 3: 0}, 5: {0: 0, 1: 0, 3: 0}, } # {id: {pos_index: angle}} - self.relais = {0: 0, 1: 0, 2: 0} - self.ascenseur_pos = 0 - self.fin_course = {} # à implémenter si besoin - self.tirette_arm = False + # State identical to STM32 getters + self.servo_angles = {i: 0.0 for i in range(16)} # angle as float + self.relay_state = {i: 0 for i in range(16)} # 0/1 + self.tirette_armed = False self.thread = threading.Thread(target=self.run) self.thread.start() - def send_response(self, msg): - print(f"[TX] {msg}") + def tx(self, msg): + print(f"[TX] {msg.strip()}") self.ser.write((msg + "\n").encode()) - def process_command(self, line): - parts = line.strip().split(';') - if not parts: + # ---------------------------------------------------------------------- + # STM32-LIKE FUNCTIONS + # ---------------------------------------------------------------------- + def getServoAngle(self, sid): + return float(self.servo_angles.get(sid, 0.0)) + + def getRelayState(self, rid): + return int(self.relay_state.get(rid, 0)) + + def moveServo(self, sid, angle): + self.servo_angles[sid] = float(angle) + + def moveRelay(self, rid, state): + self.relay_state[rid] = 1 if int(state) else 0 + + # ---------------------------------------------------------------------- + # COMMAND PARSER (as close as possible to STM32 version) + # ---------------------------------------------------------------------- + def process(self, line): + print(f"[RX] {line}") + parts = line.split(";") + if len(parts) < 2: return cmd = parts[0] @@ -39,80 +52,143 @@ class SimulatedPCB: self.handle_set(parts) elif cmd == "MOV": self.handle_mov(parts) + elif cmd == "ACK": + # simulate ACK reception ignore for now + pass - def handle_get(self, parts): - if len(parts) != 3: + # ---------------------------------------------------------------------- + # GET handler (SERVO POS / RELAY STATE) + # ---------------------------------------------------------------------- + def handle_get(self, p): + # GET;SERVO;POS;n;id... + if p[1] == "SERVO" and p[2] == "POS": + if len(p) < 4: + self.tx("KO;SET;SERVO;POS") + return + + n = int(p[3]) + if n <= 0: + self.tx("SET;SERVO;POS;0") + return + + resp = f"SET;SERVO;POS;{n}" + idx = 4 + for _ in range(n): + if idx >= len(p): + self.tx("KO;SET;SERVO;POS") + return + sid = int(p[idx]) + idx += 1 + angle = self.getServoAngle(sid) + resp += f";{sid};{angle:.4f}" + + self.tx(resp) return - category, data = parts[1], parts[2] + # GET;RELAY;STATE;n;id... + if p[1] == "RELAY" and p[2] == "STATE": + if len(p) < 4: + self.tx("KO;SET;RELAY;STATE") + return + n = int(p[3]) + if n <= 0: + self.tx("SET;RELAY;STATE;0") + return - if category.startswith("SERVO") and data == "POS": - sid = int(category[5:]) - pos = self.servos.get(sid, 0) - self.send_response(f"SET;{category};{data};{pos}") - elif category == "ASC" and data == "POS": - self.send_response(f"SET;ASC;POS;{self.ascenseur_pos}") - elif category.startswith("RELAY") and data == "STATE": - rid = int(category[5:]) - state = self.relais.get(rid, 0) - self.send_response(f"SET;{category};{data};{state}") + resp = f"SET;RELAY;STATE;{n}" + idx = 4 + for _ in range(n): + if idx >= len(p): + self.tx("KO;SET;RELAY;STATE") + return + rid = int(p[idx]) + idx += 1 + state = self.getRelayState(rid) + resp += f";{rid};{state}" - def handle_set(self, parts): - if len(parts) != 4: + self.tx(resp) return - category, data, val = parts[1], parts[2], parts[3] + # ---------------------------------------------------------------------- + # SET handler (only TIR;ARM) + # ---------------------------------------------------------------------- + def handle_set(self, p): + if p[1] == "TIR" and p[2] == "ARM": + if len(p) < 4: + self.tx("KO;TIR;ARM") + return - if category.startswith("SERVO") and data.startswith("POS"): - sid = int(category[5:]) - pos_index = int(data[3:]) - angle = float(val) - - if sid not in self.servo_angles: - self.servo_angles[sid] = {} - self.servo_angles[sid][pos_index] = angle - - self.send_response(f"OK;{category};{data};{val}") - elif category == "ASC": - self.send_response(f"OK;ASC;{data};{val}") - else: - self.send_response(f"KO;{category};{data};{val}") - - def handle_mov(self, parts): - if len(parts) != 3: + val = int(p[3]) + self.tirette_armed = bool(val) + self.tx(f"OK;TIR;ARM;{val}") return - category, data = parts[1], parts[2] + self.tx(f"KO;{p[1]};{p[2]}") - if category == "ASC": - self.ascenseur_pos = data - self.send_response(f"OK;{category};{data}") - time.sleep(.5) - elif category.startswith("SERVO") and data.startswith("POS"): - sid = int(category[5:]) - pos_index = int(data[3:]) - if sid in self.servo_angles and pos_index in self.servo_angles[sid]: - time.sleep(0.1) - self.servos[sid] = pos_index - self.send_response(f"OK;{category};{data}") - else: - self.send_response(f"KO;{category};{data}") - elif category.startswith("RELAY"): - rid = int(category[5:]) - self.relais[rid] = 1 if data == "1" else 0 - time.sleep(.1) - self.send_response(f"OK;{category};{data}") - else: - self.send_response(f"KO;{category};{data}") + # ---------------------------------------------------------------------- + # MOV handler (SERVO multi / RELAY multi) + # ---------------------------------------------------------------------- + def handle_mov(self, p): + # MOV;SERVO;n;id;angle;id;angle... + if p[1] == "SERVO": + n = int(p[2]) + idx = 3 + resp = f"OK;SERVO;{n}" + for _ in range(n): + if idx + 1 >= len(p): + self.tx(f"KO;MOV;SERVO;{p[2]}") + return + + sid = int(p[idx]) + ang = float(p[idx+1]) + idx += 2 + + self.moveServo(sid, ang) + resp += f";{sid};{ang}" + + self.tx(resp) + return + + # MOV;RELAY;n;id;state... + if p[1] == "RELAY": + n = int(p[2]) + idx = 3 + + resp = f"OK;RELAY;{n}" + for _ in range(n): + if idx + 1 >= len(p): + self.tx(f"KO;MOV;RELAY;{p[2]}") + return + + rid = int(p[idx]) + st = int(p[idx+1]) + idx += 2 + + self.moveRelay(rid, st) + resp += f";{rid};{st}" + + self.tx(resp) + return + + self.tx(f"KO;MOV;{p[1]}") + + # ---------------------------------------------------------------------- + # Loop + # ---------------------------------------------------------------------- def run(self): + buf = b"" while self.running: - if self.ser.in_waiting: - msg = self.ser.readline().decode().strip() - print(f"[RX] {msg}") - self.process_command(msg) - - time.sleep(0.1) + try: + data = self.ser.read(64) + if data: + buf += data + while b"\n" in buf: + line, buf = buf.split(b"\n", 1) + self.process(line.decode().strip()) + except Exception as e: + print(f"Serial error: {e}") + time.sleep(0.2) def stop(self): self.running = False @@ -120,18 +196,19 @@ class SimulatedPCB: self.ser.close() -if __name__ == '__main__': - parser = argparse.ArgumentParser(description="Simulated PCB") - parser.add_argument('--port', type=str, default='/tmp/USB_ACTION_SIM', help='Serial port to use') +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--port", type=str, default="/tmp/ACTION_SIM") args = parser.parse_args() - sim = None try: - sim = SimulatedPCB(port=args.port) + sim = SimulatedPCB(args.port) + while True: + time.sleep(1) except KeyboardInterrupt: if sim: sim.stop() - print("Simulation stopped") + print("Stopped.") # socat -d -d pty,raw,echo=0,link=/tmp/ACTION_SIM pty,raw,echo=0,link=/tmp/ACTION_USB # python3 simulated_pcb/action.py --port /tmp/ACTION_SIM diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index f0145a4..3361c37 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -80,8 +80,8 @@ class SimulatedPCB: self.theta += self.vtheta * dt self.theta = self.normalize_angle(self.theta) - if now - self.last_send > 0.1: - # print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}') + if now - self.last_send > 0.1 and self.start: + print(f'[TX] SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}') self.ser.write(f'SET;POS;{int(self.x)};{int(self.y)};{self.theta:.5f}\n'.encode()) self.last_send = now @@ -116,7 +116,6 @@ class SimulatedPCB: self.ser.write(f'SET;PID;{self.pid[0]};{self.pid[1]};{self.pid[2]}\n'.encode()) elif msg.startswith("SET;PID"): - self.pid = list(map(float, msg.split(';')[2:5])) print(f'[TX] OK;PID;1') self.ser.write(f'OK;PID;1\n'.encode()) diff --git a/simulated_pcb/serial.modelec.py b/simulated_pcb/serial.modelec.py index 337a4fd..ef22fa6 100644 --- a/simulated_pcb/serial.modelec.py +++ b/simulated_pcb/serial.modelec.py @@ -3,13 +3,23 @@ import threading import time import curses import argparse -import sys +import serial.tools.list_ports stop_thread = False +def list_serial_ports(): + """List all available serial ports""" + ports = serial.tools.list_ports.comports() + if not ports: + print("No serial ports found.") + else: + print("Available serial ports:") + for port in ports: + print(f" {port.device} - {port.description}") + exit(0) def read_serial(ser, log_lines, log_lock): - """Thread de lecture du port série""" + """Thread to read serial output""" global stop_thread while not stop_thread: if ser.in_waiting: @@ -22,11 +32,10 @@ def read_serial(ser, log_lines, log_lock): log_lines.pop(0) except Exception as e: with log_lock: - log_lines.append(f"[Erreur série] {e}") + log_lines.append(f"[Serial Error] {e}") time.sleep(0.05) - -def curses_main(stdscr, serial_port, baudrate): +def curses_main(stdscr, port, baudrate): global stop_thread curses.curs_set(1) stdscr.nodelay(True) @@ -35,19 +44,9 @@ def curses_main(stdscr, serial_port, baudrate): log_lines = [] log_lock = threading.Lock() cmd_history = [] + history_index = -1 # For navigating command history - # --- Try to open the serial port --- - try: - ser = serial.Serial(serial_port, baudrate, timeout=1) - except serial.SerialException as e: - stdscr.clear() - stdscr.addstr(0, 0, f"[Erreur] Impossible d'ouvrir le port {serial_port}: {e}") - stdscr.addstr(2, 0, "Appuyez sur une touche pour quitter.") - stdscr.refresh() - stdscr.getch() - return - - with ser: + with serial.Serial(port, baudrate, timeout=1) as ser: time.sleep(2) reader_thread = threading.Thread(target=read_serial, args=(ser, log_lines, log_lock), daemon=True) reader_thread.start() @@ -58,11 +57,11 @@ def curses_main(stdscr, serial_port, baudrate): stdscr.clear() h, w = stdscr.getmaxyx() - # Split screen horizontally (logs left, history right) + # Split screen horizontally (70% logs, 30% command history) split_x = int(w * 0.7) log_height = h - 2 - # --- Left panel: serial logs --- + # --- Left panel: logs --- with log_lock: visible_logs = log_lines[-log_height:] for i, line in enumerate(visible_logs): @@ -71,13 +70,15 @@ def curses_main(stdscr, serial_port, baudrate): # --- Right panel: command history --- stdscr.vline(0, split_x, "|", log_height) history_start_x = split_x + 2 - stdscr.addstr(0, history_start_x, "Historique des commandes:") - for i, cmd in enumerate(reversed(cmd_history[-(log_height - 2):])): + stdscr.addstr(0, history_start_x, "Command History:") + + visible_history = cmd_history[-(log_height - 2):] + for i, cmd in enumerate(visible_history): stdscr.addnstr(i + 1, history_start_x, cmd, w - history_start_x - 1) - # --- Bottom input line --- + # --- Input line --- stdscr.addstr(log_height, 0, "-" * (w - 1)) - stdscr.addstr(log_height + 1, 0, f"Commande >>> {user_input}") + stdscr.addstr(log_height + 1, 0, f"Command >>> {user_input}") stdscr.refresh() try: @@ -88,23 +89,42 @@ def curses_main(stdscr, serial_port, baudrate): if ch is None: continue - # Handle key input if isinstance(ch, str): - if ch in ("\n", "\r"): # Enter + if ch in ("\n", "\r"): # Enter key cmd = user_input.strip() if cmd.lower() in ("exit", "quit"): stop_thread = True break - if cmd: + elif cmd.lower() == "clear": + with log_lock: + log_lines.clear() + elif cmd: ser.write((cmd + "\n").encode()) cmd_history.append(cmd) + history_index = -1 user_input = "" - elif ch in ("\b", "\x7f"): # Backspace + elif ch in ("\b", "\x7f"): user_input = user_input[:-1] elif ch.isprintable(): user_input += ch elif ch == curses.KEY_BACKSPACE: user_input = user_input[:-1] + elif ch == curses.KEY_UP: + if cmd_history: + if history_index == -1: + history_index = len(cmd_history) - 1 + elif history_index > 0: + history_index -= 1 + user_input = cmd_history[history_index] + elif ch == curses.KEY_DOWN: + if cmd_history: + if history_index != -1: + history_index += 1 + if history_index >= len(cmd_history): + history_index = -1 + user_input = "" + else: + user_input = cmd_history[history_index] elif ch == 27: # ESC stop_thread = True break @@ -114,23 +134,15 @@ def curses_main(stdscr, serial_port, baudrate): def main(): - parser = argparse.ArgumentParser( - description="Terminal série interactif avec logs et historique des commandes." - ) - parser.add_argument( - "--port", - type=str, - default="/dev/USB_ODO", - help="Port série à utiliser (ex: /dev/ttyUSB0, COM3, etc.)", - ) - parser.add_argument( - "--baudrate", - type=int, - default=115200, - help="Vitesse de communication (baudrate, ex: 9600, 115200, etc.)", - ) + parser = argparse.ArgumentParser(description="Serial monitor with command history and curses UI.") + parser.add_argument("--port", default="/dev/USB_ODO", help="Serial port to use (default: /dev/USB_ODO)") + parser.add_argument("--baudrate", type=int, default=115200, help="Baud rate (default: 115200)") + parser.add_argument("--list", action="store_true", help="List available serial ports and exit") args = parser.parse_args() + if args.list: + list_serial_ports() + curses.wrapper(curses_main, args.port, args.baudrate) diff --git a/simulated_pcb/serial.modelec.test.py b/simulated_pcb/serial.modelec.test.py deleted file mode 100644 index ef22fa6..0000000 --- a/simulated_pcb/serial.modelec.test.py +++ /dev/null @@ -1,150 +0,0 @@ -import serial -import threading -import time -import curses -import argparse -import serial.tools.list_ports - -stop_thread = False - -def list_serial_ports(): - """List all available serial ports""" - ports = serial.tools.list_ports.comports() - if not ports: - print("No serial ports found.") - else: - print("Available serial ports:") - for port in ports: - print(f" {port.device} - {port.description}") - exit(0) - -def read_serial(ser, log_lines, log_lock): - """Thread to read serial output""" - global stop_thread - while not stop_thread: - if ser.in_waiting: - try: - line = ser.readline().decode(errors='ignore').strip() - if line: - with log_lock: - log_lines.append(line) - if len(log_lines) > 1000: - log_lines.pop(0) - except Exception as e: - with log_lock: - log_lines.append(f"[Serial Error] {e}") - time.sleep(0.05) - -def curses_main(stdscr, port, baudrate): - global stop_thread - curses.curs_set(1) - stdscr.nodelay(True) - stdscr.timeout(100) - - log_lines = [] - log_lock = threading.Lock() - cmd_history = [] - history_index = -1 # For navigating command history - - with serial.Serial(port, baudrate, timeout=1) as ser: - time.sleep(2) - reader_thread = threading.Thread(target=read_serial, args=(ser, log_lines, log_lock), daemon=True) - reader_thread.start() - - user_input = "" - - while True: - stdscr.clear() - h, w = stdscr.getmaxyx() - - # Split screen horizontally (70% logs, 30% command history) - split_x = int(w * 0.7) - log_height = h - 2 - - # --- Left panel: logs --- - with log_lock: - visible_logs = log_lines[-log_height:] - for i, line in enumerate(visible_logs): - stdscr.addnstr(i, 0, line, split_x - 1) - - # --- Right panel: command history --- - stdscr.vline(0, split_x, "|", log_height) - history_start_x = split_x + 2 - stdscr.addstr(0, history_start_x, "Command History:") - - visible_history = cmd_history[-(log_height - 2):] - for i, cmd in enumerate(visible_history): - stdscr.addnstr(i + 1, history_start_x, cmd, w - history_start_x - 1) - - # --- Input line --- - stdscr.addstr(log_height, 0, "-" * (w - 1)) - stdscr.addstr(log_height + 1, 0, f"Command >>> {user_input}") - stdscr.refresh() - - try: - ch = stdscr.get_wch() - except curses.error: - ch = None - - if ch is None: - continue - - if isinstance(ch, str): - if ch in ("\n", "\r"): # Enter key - cmd = user_input.strip() - if cmd.lower() in ("exit", "quit"): - stop_thread = True - break - elif cmd.lower() == "clear": - with log_lock: - log_lines.clear() - elif cmd: - ser.write((cmd + "\n").encode()) - cmd_history.append(cmd) - history_index = -1 - user_input = "" - elif ch in ("\b", "\x7f"): - user_input = user_input[:-1] - elif ch.isprintable(): - user_input += ch - elif ch == curses.KEY_BACKSPACE: - user_input = user_input[:-1] - elif ch == curses.KEY_UP: - if cmd_history: - if history_index == -1: - history_index = len(cmd_history) - 1 - elif history_index > 0: - history_index -= 1 - user_input = cmd_history[history_index] - elif ch == curses.KEY_DOWN: - if cmd_history: - if history_index != -1: - history_index += 1 - if history_index >= len(cmd_history): - history_index = -1 - user_input = "" - else: - user_input = cmd_history[history_index] - elif ch == 27: # ESC - stop_thread = True - break - - stop_thread = True - reader_thread.join(timeout=1) - - -def main(): - parser = argparse.ArgumentParser(description="Serial monitor with command history and curses UI.") - parser.add_argument("--port", default="/dev/USB_ODO", help="Serial port to use (default: /dev/USB_ODO)") - parser.add_argument("--baudrate", type=int, default=115200, help="Baud rate (default: 115200)") - parser.add_argument("--list", action="store_true", help="List available serial ports and exit") - args = parser.parse_args() - - if args.list: - list_serial_ports() - - curses.wrapper(curses_main, args.port, args.baudrate) - - -if __name__ == "__main__": - main() diff --git a/src/modelec_com/CMakeLists.txt b/src/modelec_com/CMakeLists.txt index b98c1e7..2e65e3d 100644 --- a/src/modelec_com/CMakeLists.txt +++ b/src/modelec_com/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(ament_index_cpp REQUIRED) +find_package(fmt) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) @@ -27,7 +28,7 @@ target_include_directories(pcb_odo_interface PUBLIC # PCB Action Node add_executable(pcb_action_interface src/pcb_action_interface.cpp src/serial_listener.cpp) ament_target_dependencies(pcb_action_interface rclcpp std_msgs modelec_interfaces ament_index_cpp) -target_link_libraries(pcb_action_interface Boost::system modelec_utils::utils modelec_utils::config) +target_link_libraries(pcb_action_interface Boost::system modelec_utils::utils modelec_utils::config fmt::fmt) target_include_directories(pcb_action_interface PUBLIC $ $ diff --git a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp index d3199d3..f73aa0c 100644 --- a/src/modelec_com/include/modelec_com/pcb_action_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_action_interface.hpp @@ -6,13 +6,25 @@ #include #include #include -#include #include -#include -#include +#include +#include +#include +#include + +#define TIMER_SERVO_TIMED_MS 10 // 100 Hz namespace Modelec { + struct ServoTimedSet + { + modelec_interfaces::msg::ActionServoTimed servo_timed; + rclcpp::Time start_time; + rclcpp::Time end_time; + + bool active = false; + }; + class PCBActionInterface : public rclcpp::Node, public SerialListener { public: @@ -23,36 +35,38 @@ namespace Modelec protected: std::map asc_value_mapper_; - std::map> servo_pos_mapper_; int asc_state_ = 0; - std::map servo_value_; + std::map servo_value_; std::map relay_value_; + std::map servo_timed_buffer_; + + rclcpp::TimerBase::SharedPtr servo_timed_timer_; + private: void read(const std::string& msg) override; rclcpp::Subscription::SharedPtr asc_get_sub_; - rclcpp::Subscription::SharedPtr servo_get_sub_; - rclcpp::Subscription::SharedPtr relay_get_sub_; - rclcpp::Publisher::SharedPtr asc_get_res_pub_; - rclcpp::Publisher::SharedPtr servo_get_res_pub_; - rclcpp::Publisher::SharedPtr relay_get_res_pub_; - rclcpp::Subscription::SharedPtr asc_set_sub_; - rclcpp::Subscription::SharedPtr servo_set_sub_; - rclcpp::Publisher::SharedPtr asc_set_res_pub_; - rclcpp::Publisher::SharedPtr servo_set_res_pub_; - rclcpp::Subscription::SharedPtr asc_move_sub_; - rclcpp::Subscription::SharedPtr servo_move_sub_; - rclcpp::Subscription::SharedPtr relay_move_sub_; - rclcpp::Publisher::SharedPtr asc_move_res_pub_; - rclcpp::Publisher::SharedPtr servo_move_res_pub_; - rclcpp::Publisher::SharedPtr relay_move_res_pub_; + + rclcpp::Subscription::SharedPtr servo_get_sub_; + rclcpp::Publisher::SharedPtr servo_get_res_pub_; + rclcpp::Subscription::SharedPtr servo_move_sub_; + rclcpp::Publisher::SharedPtr servo_move_res_pub_; + + rclcpp::Subscription::SharedPtr servo_move_timed_sub_; + rclcpp::Publisher::SharedPtr servo_move_timed_res_pub_; + + rclcpp::Subscription::SharedPtr relay_get_sub_; + rclcpp::Publisher::SharedPtr relay_get_res_pub_; + rclcpp::Subscription::SharedPtr relay_move_sub_; + rclcpp::Publisher::SharedPtr relay_move_res_pub_; + rclcpp::Publisher::SharedPtr tir_start_pub_; rclcpp::Publisher::SharedPtr tir_arm_pub_; @@ -68,6 +82,7 @@ namespace Modelec void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}); + // TODO redo thos func to accept arrays, poc without them atm void GetData(const std::string& elem, const std::vector& data = {}); void SendOrder(const std::string& elem, const std::vector& data = {}); void SendMove(const std::string& elem, const std::vector& data = {}); diff --git a/src/modelec_com/src/pcb_action_interface.cpp b/src/modelec_com/src/pcb_action_interface.cpp index be79c8d..320c24b 100644 --- a/src/modelec_com/src/pcb_action_interface.cpp +++ b/src/modelec_com/src/pcb_action_interface.cpp @@ -2,10 +2,11 @@ #include #include #include +#include namespace Modelec { - PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener() + PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface") { // Service to create a new serial listener declare_parameter("serial_port", "/dev/USB_ACTION"); @@ -17,8 +18,6 @@ namespace Modelec request->bauds = get_parameter("baudrate").as_int(); request->serial_port = get_parameter("serial_port").as_string(); - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); - asc_get_sub_ = this->create_subscription( "action/get/asc", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr) @@ -26,25 +25,37 @@ namespace Modelec GetData("ASC", {"POS"}); }); - servo_get_sub_ = this->create_subscription( + servo_get_sub_ = this->create_subscription( "action/get/servo", 10, - [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg) { - GetData("SERVO" + std::to_string(msg->id), {"POS"}); + std::string data = "GET;SERVO;POS;" + std::to_string(msg->items.size()) + ";"; + for (const auto& item : msg->items) { + data += std::to_string(item.id) + ";"; + } + data += "\n"; + + SendToPCB(data); }); - relay_get_sub_ = this->create_subscription( + relay_get_sub_ = this->create_subscription( "action/get/relay", 10, - [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) + [this](const modelec_interfaces::msg::ActionRelayStateArray::SharedPtr msg) { - GetData("RELAY" + std::to_string(msg->id), {"STATE"}); + std::string data = "GET;RELAY;STATE;" + std::to_string(msg->items.size()) + ";"; + for (const auto& item : msg->items) { + data += std::to_string(item.id) + ";"; + } + data += "\n"; + + SendToPCB(data); }); asc_get_res_pub_ = this->create_publisher( "action/get/asc/res", 10); - servo_get_res_pub_ = this->create_publisher( + servo_get_res_pub_ = this->create_publisher( "action/get/servo/res", 10); - relay_get_res_pub_ = this->create_publisher( + relay_get_res_pub_ = this->create_publisher( "action/get/relay/res", 10); asc_set_sub_ = this->create_subscription( @@ -54,21 +65,9 @@ namespace Modelec SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)}); }); - servo_set_sub_ = this->create_subscription( - "action/set/servo", 10, - [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) - { - SendOrder("SERVO" + std::to_string(msg->id), { - "POS" + std::to_string(msg->pos), std::to_string(static_cast(msg->angle * 100)) - }); - }); - asc_set_res_pub_ = this->create_publisher( "action/set/asc/res", 10); - servo_set_res_pub_ = this->create_publisher( - "action/set/servo/res", 10); - asc_move_sub_ = this->create_subscription( "action/move/asc", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg) @@ -76,27 +75,41 @@ namespace Modelec SendMove("ASC", {std::to_string(msg->pos)}); }); - servo_move_sub_ = this->create_subscription( + servo_move_sub_ = this->create_subscription( "action/move/servo", 10, - [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg) + [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg) { - SendMove("SERVO" + std::to_string(msg->id), {"POS" + std::to_string(msg->pos)}); + std::string data = "MOV;SERVO;" + std::to_string(msg->items.size()) + ";"; + for (const auto& item : msg->items) + { + data += std::to_string(item.id) + ";" + fmt::format("{:.3f}", item.angle) + ";"; + } + data += "\n"; + + SendToPCB(data); }); - relay_move_sub_ = this->create_subscription( + relay_move_sub_ = this->create_subscription( "action/move/relay", 10, - [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg) + [this](const modelec_interfaces::msg::ActionRelayStateArray::SharedPtr msg) { - SendMove("RELAY" + std::to_string(msg->id), {std::to_string(msg->state)}); + std::string data = "MOV;RELAY;STATE;" + std::to_string(msg->items.size()) + ";"; + for (const auto& item : msg->items) + { + data += std::to_string(item.id) + ";" + std::to_string(item.state) + ";"; + } + data += "\n"; + + SendToPCB(data); }); asc_move_res_pub_ = this->create_publisher( "action/move/asc/res", 10); - servo_move_res_pub_ = this->create_publisher( + servo_move_res_pub_ = this->create_publisher( "action/move/servo/res", 10); - relay_move_res_pub_ = this->create_publisher( + relay_move_res_pub_ = this->create_publisher( "action/move/relay/res", 10); tir_start_pub_ = this->create_publisher( @@ -137,6 +150,91 @@ namespace Modelec }); + servo_move_timed_sub_ = this->create_subscription( + "action/move/servo/timed", 10, + [this](const modelec_interfaces::msg::ActionServoTimedArray::SharedPtr msg) + { + rclcpp::Time now = this->now(); + + for (const auto& item : msg->items) + { + ServoTimedSet servo_timed_set; + servo_timed_set.servo_timed = item; + servo_timed_set.start_time = now; + servo_timed_set.end_time = now + rclcpp::Duration::from_seconds(item.duration_s); + servo_timed_set.active = true; + + RCLCPP_DEBUG(this->get_logger(), "Scheduled timed move for Servo ID %d from %.3f to %.3f over %.3f seconds", + item.id, item.start_angle, item.end_angle, item.duration_s); + + servo_timed_buffer_[item.id] = servo_timed_set; + } + }); + + servo_move_timed_res_pub_ = this->create_publisher( + "action/move/servo/timed/res", 10); + + servo_timed_timer_ = this->create_wall_timer( + std::chrono::milliseconds(TIMER_SERVO_TIMED_MS), + [this]() + { + rclcpp::Time now = this->now(); + + modelec_interfaces::msg::ActionServoTimedArray servo_timed_msg; + + std::map to_send; + + for (auto& [id, servo_timed_set] : servo_timed_buffer_) + { + if (servo_timed_set.active && now.nanoseconds() >= servo_timed_set.end_time.nanoseconds()) + { + RCLCPP_DEBUG(this->get_logger(), "Timed move for Servo ID %d completed. Setting to final angle %.3f", + id, servo_timed_set.servo_timed.end_angle); + + to_send[id] = servo_timed_set.servo_timed.end_angle; + servo_timed_set.active = false; + + servo_timed_msg.items.push_back(servo_timed_set.servo_timed); + } + else if (servo_timed_set.active) + { + double elapsed = (now - servo_timed_set.start_time).seconds(); + double duration = (servo_timed_set.end_time - servo_timed_set.start_time).seconds(); + double progress = elapsed / duration; + + RCLCPP_DEBUG(this->get_logger(), "Servo ID %d progress: %.3f | %.3f %.3f", id, progress, elapsed, duration); + + double intermediate_angle = servo_timed_set.servo_timed.start_angle + + progress * (servo_timed_set.servo_timed.end_angle - servo_timed_set.servo_timed.start_angle); + + to_send[id] = intermediate_angle; + } + } + + if (!to_send.empty()) + { + std::string data = "MOV;SERVO;" + std::to_string(to_send.size()) + ";"; + + for (const auto& [id, angle] : to_send) + { + data += std::to_string(id) + ";" + fmt::format("{:.3f}", angle) + ";"; + } + + data += "\n"; + + SendToPCB(data); + } + + if (!servo_timed_msg.items.empty()) + { + servo_timed_msg.success = true; + servo_move_timed_res_pub_->publish(servo_timed_msg); + } + + }); + + this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); + // TODO : check for real value there /*asc_value_mapper_ = { {0, 0}, @@ -153,49 +251,55 @@ namespace Modelec // SendMove("ASC", {std::to_string(asc_state_)}); - servo_pos_mapper_ = { - {0, {{0, 0.55}, {1, 0}}}, - {1, {{0, 0}, {1, 0.4}}}, - {2, {{0, M_PI_2}}}, - {3, {{0, M_PI_2}}}, - {4, {{0, 1.25}, {1, 0.45}}}, - {5, {{0, 0}, {1, M_PI}}}, - }; + // rclcpp::sleep_for(std::chrono::milliseconds(100)); - for (auto & [id, v] : servo_pos_mapper_) - { - if (id == 5) continue; - - for (auto & [key, angle] : v) - { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(static_cast(angle * 100))}); - } - } - - servo_value_ = { + /*servo_value_ = { {0, 1}, {1, 1}, {2, 0}, {3, 0}, {4, 1}, {5, 0} + };*/ + + servo_value_ = { + {0, 0}, + {1, 3}, + {2, 3}, + {3, 0}, + {4, 1.57}, + {5, 1.4}, }; + std::string data = "MOV;SERVO;" + std::to_string(servo_value_.size()) + ";"; + for (auto & [id, v] : servo_value_) { - rclcpp::sleep_for(std::chrono::milliseconds(100)); - - SendMove("SERVO" + std::to_string(id), {"POS" + std::to_string(v)}); + data += std::to_string(id) + ";" + fmt::format("{:.3f}", v) + ";"; } + data += "\n"; + + SendToPCB(data); + /*relay_value_ = { {1, false}, {2, false}, {3, false}, };*/ + /*rclcpp::sleep_for(std::chrono::milliseconds(100)); + + data = "SET;RELAY;STATE;"; + data += std::to_string(relay_value_.size()) + ";"; + + for (auto & [id, v] : relay_value_) + { + data += std::to_string(id) + ";" + std::to_string(v) + ";"; + } + + SendToPCB(data);*/ + /*for (auto & [id, v] : relay_value_) { rclcpp::sleep_for(std::chrono::milliseconds(100)); @@ -210,8 +314,9 @@ namespace Modelec void PCBActionInterface::read(const std::string& msg) { - RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str()); - RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", msg.c_str()); + // RCLCPP_INFO(this->get_logger(), "Received message: '%s'", msg.c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", trim(msg).c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", trim(msg).c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) @@ -232,37 +337,52 @@ namespace Modelec asc_msg.success = true; asc_get_res_pub_->publish(asc_msg); } - else if (startsWith(tokens[1], "SERVO")) + else if (tokens[1] == "SERVO") { - int servo_id = std::stoi(tokens[1].substr(5)); - int v = std::stoi(tokens[3]); - servo_value_[servo_id] = v; + int n = std::stoi(tokens[2]); + modelec_interfaces::msg::ActionServoPosArray servo_msg; + servo_msg.items.resize(n); - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.id = servo_id; - servo_msg.pos = v; - servo_msg.angle = servo_pos_mapper_[servo_id][v]; + for (int i = 0; i < n; ++i) + { + int servo_id = std::stoi(tokens[3 + i * 2]); + int angle = std::stoi(tokens[4 + i * 2]); + + servo_msg.items[i].id = servo_id; + servo_msg.items[i].angle = angle; + servo_msg.items[i].success = true; + } servo_msg.success = true; + servo_get_res_pub_->publish(servo_msg); } - else if (startsWith(tokens[1], "RELAY")) + else if (tokens[1] == "RELAY") { - int relay_id = std::stoi(tokens[1].substr(5)); - bool state = (tokens[3] == "1"); - relay_value_[relay_id] = state; + int n = std::stoi(tokens[2]); + modelec_interfaces::msg::ActionRelayStateArray relay_msg; + relay_msg.items.resize(n); - modelec_interfaces::msg::ActionRelayState relay_msg; - relay_msg.id = relay_id; - relay_msg.state = state; + for (int i = 0; i < n; ++i) + { + int relay_id = std::stoi(tokens[3 + i * 2]); + bool state = (tokens[4 + i * 2] == "1"); + relay_value_[relay_id] = state; + + relay_msg.items[i].id = relay_id; + relay_msg.items[i].state = state; + relay_msg.items[i].success = true; + } relay_msg.success = true; + relay_get_res_pub_->publish(relay_msg); } } else if (tokens[0] == "OK") { - if (tokens.size() == 4) + // TODO rework this part with the pcb protocol + if (tokens[1] == "ASC") { - if (tokens[1] == "ASC") + if (tokens.size() == 4) { int pos = std::stoi(tokens[2]); int v = std::stoi(tokens[3]); @@ -274,24 +394,7 @@ namespace Modelec asc_msg.success = true; asc_set_res_pub_->publish(asc_msg); } - else if (startsWith(tokens[1], "SERVO")) - { - int servo_id = std::stoi(tokens[1].substr(5)); - int key = std::stoi(tokens[2].substr(3)); - int v = std::stoi(tokens[3]); - servo_pos_mapper_[servo_id][key] = v; - - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.id = servo_id; - servo_msg.pos = key; - servo_msg.angle = v; - servo_msg.success = true; - servo_set_res_pub_->publish(servo_msg); - } - } - else if (tokens.size() == 3) - { - if (tokens[1] == "ASC") + else { asc_state_ = std::stoi(tokens[2]); @@ -300,77 +403,84 @@ namespace Modelec asc_msg.success = true; asc_move_res_pub_->publish(asc_msg); } - else if (startsWith(tokens[1], "SERVO")) - { - int servo_id = std::stoi(tokens[1].substr(5)); - int key = std::stoi(tokens[2].substr(3)); - servo_value_[servo_id] = key; + } + else if (tokens[1] == "SERVO") + { + int n = std::stoi(tokens[2]); + modelec_interfaces::msg::ActionServoPosArray servo_msg; + servo_msg.items.resize(n); - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.id = servo_id; - servo_msg.pos = key; - servo_msg.success = true; - servo_move_res_pub_->publish(servo_msg); - } - else if (startsWith(tokens[1], "RELAY")) + for (int i = 0; i < n; ++i) { - int relay_id = std::stoi(tokens[1].substr(5)); - bool state = (tokens[2] == "1"); + int servo_id = std::stoi(tokens[3 + i * 2]); + float angle = std::stof(tokens[4 + i * 2]); + servo_value_[servo_id] = angle; + + servo_msg.items[i].id = servo_id; + servo_msg.items[i].angle = angle; + servo_msg.items[i].success = true; + } + servo_msg.success = true; + + servo_move_res_pub_->publish(servo_msg); + } + else if (tokens[1] == "RELAY") + { + int n = std::stoi(tokens[2]); + modelec_interfaces::msg::ActionRelayStateArray relay_msg; + relay_msg.items.resize(n); + + for (int i = 0; i < n; ++i) + { + int relay_id = std::stoi(tokens[3 + i * 2]); + bool state = (tokens[4 + i * 2] == "1"); relay_value_[relay_id] = state; - modelec_interfaces::msg::ActionRelayState relay_msg; - relay_msg.id = relay_id; - relay_msg.state = state; - relay_msg.success = true; - relay_move_res_pub_->publish(relay_msg); + relay_msg.items[i].id = relay_id; + relay_msg.items[i].state = state; + relay_msg.items[i].success = true; } + relay_msg.success = true; + + relay_move_res_pub_->publish(relay_msg); } else { - RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg.c_str()); + RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", trim(msg).c_str()); } } else if (tokens[0] == "KO") { - if (tokens.size() == 4) + if (tokens[1] == "ASC") { - if (tokens[1] == "ASC") + if (tokens.size() == 4) { modelec_interfaces::msg::ActionAscPos asc_msg; asc_msg.success = false; asc_set_res_pub_->publish(asc_msg); } - else if (startsWith(tokens[1], "SERVO")) - { - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.success = false; - servo_set_res_pub_->publish(servo_msg); - } - } - else if (tokens.size() == 3) - { - if (tokens[1] == "ASC") + else { modelec_interfaces::msg::ActionAscPos asc_msg; asc_msg.success = false; asc_move_res_pub_->publish(asc_msg); } - else if (startsWith(tokens[1], "SERVO")) - { - modelec_interfaces::msg::ActionServoPos servo_msg; - servo_msg.success = false; - servo_move_res_pub_->publish(servo_msg); - } - else if (startsWith(tokens[1], "RELAY")) - { - modelec_interfaces::msg::ActionRelayState relay_msg; - relay_msg.success = false; - relay_move_res_pub_->publish(relay_msg); - } + } + else if (tokens[1] == "SERVO") + { + modelec_interfaces::msg::ActionServoPosArray servo_msg; + servo_msg.success = false; + servo_move_res_pub_->publish(servo_msg); + } + else if (tokens[1] == "RELAY") + { + modelec_interfaces::msg::ActionRelayStateArray relay_msg; + relay_msg.success = false; + relay_move_res_pub_->publish(relay_msg); } else { - RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg.c_str()); + RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", trim(msg).c_str()); } } else if (tokens[0] == "EVENT") @@ -400,7 +510,9 @@ namespace Modelec { if (IsOk()) { - RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); + // RCLCPP_INFO(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Sending to PCB: %s", data.c_str()); this->write(data); } } @@ -418,6 +530,7 @@ namespace Modelec SendToPCB(command); } + // TODO CHANGE TO AAA;BBB;N;X;Y;X;Y;X;Y where N is number of elem you want to send void PCBActionInterface::GetData(const std::string& elem, const std::vector& data) { SendToPCB("GET", elem, data); diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index 452e7fe..8345e7d 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -5,7 +5,7 @@ namespace Modelec { - PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener() + PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface") { declare_parameter("serial_port", "/dev/USB_ODO"); declare_parameter("baudrate", 115200); @@ -17,8 +17,6 @@ namespace Modelec request->bauds = get_parameter("baudrate").as_int(); request->serial_port = get_parameter("serial_port").as_string(); - this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN); - odo_pos_publisher_ = this->create_publisher( "odometry/position", 10); @@ -82,6 +80,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); @@ -96,12 +95,13 @@ namespace Modelec void PCBOdoInterface::read(const std::string& msg) { - RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str()); + // RCLCPP_INFO(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", trim(msg).c_str()); RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str()); std::vector tokens = split(trim(msg), ';'); if (tokens.size() < 2) { - RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg.c_str()); + RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", trim(msg).c_str()); return; } @@ -188,7 +188,7 @@ namespace Modelec } else { - RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg.c_str()); + RCLCPP_INFO(this->get_logger(), "PCB response: %s", trim(msg).c_str()); } } else if (tokens[0] == "KO") @@ -205,7 +205,7 @@ namespace Modelec } else { - RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg.c_str()); + RCLCPP_WARN(this->get_logger(), "PCB error: %s", trim(msg).c_str()); } } } @@ -233,7 +233,9 @@ namespace Modelec { if (IsOk()) { - RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str()); + // RCLCPP_INFO(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + RCLCPP_INFO_ONCE(this->get_logger(), "Sending to PCB: %s", trim(data).c_str()); + RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Sending to PCB: %s", data.c_str()); this->write(data); } } diff --git a/src/modelec_core/launch/modelec.launch.py b/src/modelec_core/launch/modelec.launch.py index 2bd954c..95e1041 100644 --- a/src/modelec_core/launch/modelec.launch.py +++ b/src/modelec_core/launch/modelec.launch.py @@ -93,13 +93,12 @@ def generate_launch_description(): def launch_com(context, *args, **kwargs): if context.launch_configurations.get('with_com') == 'true': return [ - Node(package='modelec_com', executable='serial_listener', name='serial_listener'), Node( package='modelec_com', executable='pcb_odo_interface', name='pcb_odo_interface', parameters=[{ - 'serial_port': "/dev/USB_ODO", + 'serial_port': "/tmp/USB_ODO", 'baudrate': 115200, 'name': "pcb_odo", }] @@ -109,7 +108,7 @@ def generate_launch_description(): executable='pcb_action_interface', name='pcb_action_interface', parameters=[{ - 'serial_port': "/dev/USB_ACTION", + 'serial_port': "/tmp/USB_ACTION", 'baudrate': 115200, 'name': "pcb_action", }] diff --git a/src/modelec_core/launch/test.modelec.launch.py b/src/modelec_core/launch/test.modelec.launch.py deleted file mode 100644 index 707c91d..0000000 --- a/src/modelec_core/launch/test.modelec.launch.py +++ /dev/null @@ -1,173 +0,0 @@ -from launch import LaunchDescription -from launch.actions import ( - DeclareLaunchArgument, - IncludeLaunchDescription, - OpaqueFunction, - Shutdown, - RegisterEventHandler, - TimerAction, -) -from launch.conditions import IfCondition -from launch.event_handlers import OnProcessExit -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from ament_index_python.packages import get_package_share_directory -import os - - -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?') - - 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') - scan_mode = LaunchConfiguration('scan_mode', default='Sensitivity') - - # Get launch configs - with_gui = LaunchConfiguration('with_gui') - with_rplidar = LaunchConfiguration('with_rplidar') - with_com = LaunchConfiguration('with_com') - with_strat = LaunchConfiguration('with_strat') - - # --- RPLIDAR --- - def launch_rplidar_restart_if_needed(context, *args, **kwargs): - if context.launch_configurations.get('with_rplidar') == 'true': - def create_rplidar_node(): - return Node( - package='rplidar_ros', - executable='rplidar_node', - name='rplidar_node', - parameters=[{ - 'channel_type': channel_type, - 'serial_port': serial_port, - 'serial_baudrate': serial_baudrate, - 'frame_id': frame_id, - 'inverted': inverted, - 'angle_compensate': angle_compensate - }], - output='screen' - ) - - rplidar_node = create_rplidar_node() - - restart_handler = RegisterEventHandler( - OnProcessExit( - target_action=rplidar_node, - on_exit=[ - TimerAction( - period=8.0, - actions=[create_rplidar_node()] - ) - ] - ) - ) - - return [rplidar_node, restart_handler] - return [] - - # --- 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' - ) - shutdown = RegisterEventHandler( - OnProcessExit( - target_action=gui, - on_exit=[Shutdown()] - ) - ) - return [gui, shutdown] - return [] - - # --- COM --- - def launch_com(context, *args, **kwargs): - if context.launch_configurations.get('with_com') == 'true': - serial_listener = Node( - package='modelec_com', - executable='serial_listener', - name='serial_listener' - ) - - pcb_odo_interface = Node( - package='modelec_com', - executable='pcb_odo_interface', - name='pcb_odo_interface', - parameters=[{ - 'serial_port': "/dev/USB_ODO", - 'baudrate': 115200, - 'name': "pcb_odo", - }] - ) - - pcb_action_interface = Node( - package='modelec_com', - executable='pcb_action_interface', - name='pcb_action_interface', - parameters=[{ - 'serial_port': "/dev/USB_ACTION", - 'baudrate': 115200, - 'name': "pcb_action", - }] - ) - - return [ - serial_listener, - pcb_odo_interface, - pcb_action_interface, - ] - return [] - - # --- STRATEGY --- - 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'), - ] - return [] - - # --- DELAY SETUP --- - lidar_launch = OpaqueFunction(function=launch_rplidar_restart_if_needed) - - delayed_others = TimerAction( - period=5.0, # wait 1 second after LiDAR start - actions=[ - OpaqueFunction(function=launch_gui), - OpaqueFunction(function=launch_com), - OpaqueFunction(function=launch_strat), - ] - ) - - # --- FINAL LAUNCH DESCRIPTION --- - 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'), - - with_gui_arg, - with_rplidar_arg, - with_com_arg, - with_strat_arg, - - # LiDAR first - lidar_launch, - - # Delay 1 second before others - delayed_others, - ]) diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index 1128147..967dd92 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -37,7 +37,8 @@ add_executable(modelec_gui src/pages/home_page.cpp src/pages/test_map_page.cpp src/pages/map_page.cpp - src/pages/action_page.cpp + # src/pages/action_page.cpp + src/pages/action_page.new.cpp src/pages/alim_page.cpp src/widget/action_widget.cpp @@ -46,7 +47,8 @@ add_executable(modelec_gui include/modelec_gui/pages/home_page.hpp include/modelec_gui/pages/test_map_page.hpp include/modelec_gui/pages/map_page.hpp - include/modelec_gui/pages/action_page.hpp + # include/modelec_gui/pages/action_page.hpp + include/modelec_gui/pages/action_page.new.hpp include/modelec_gui/pages/alim_page.hpp include/modelec_gui/widget/action_widget.hpp ) diff --git a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp index 3336b87..e9b721e 100644 --- a/src/modelec_gui/include/modelec_gui/modelec_gui.hpp +++ b/src/modelec_gui/include/modelec_gui/modelec_gui.hpp @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include 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 a3f01fd..8e3452e 100644 --- a/src/modelec_gui/include/modelec_gui/pages/action_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/action_page.hpp @@ -11,8 +11,8 @@ #include #include -#include -#include +#include +#include #include namespace ModelecGUI @@ -24,8 +24,8 @@ namespace ModelecGUI public: using ActionAscPos = modelec_interfaces::msg::ActionAscPos; - using ActionServoPos = modelec_interfaces::msg::ActionServoPos; - using ActionRelayState = modelec_interfaces::msg::ActionRelayState; + using ActionServoPosArray = modelec_interfaces::msg::ActionServoPosArray; + using ActionRelayStateArray = modelec_interfaces::msg::ActionRelayStateArray; using ActionExec = modelec_interfaces::msg::ActionExec; ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); @@ -54,7 +54,9 @@ namespace ModelecGUI ActionWidget* servo7_action_; ActionWidget* servo8_action_; std::vector servo_actions_; - std::vector waiting_for_move_servo_; + + QPushButton* test_button_; + bool test_ = false; QHBoxLayout* relay_layout_; @@ -76,14 +78,12 @@ namespace ModelecGUI rclcpp::Publisher::SharedPtr asc_move_pub_; rclcpp::Subscription::SharedPtr asc_move_res_sub_; - rclcpp::Subscription::SharedPtr servo_get_sub_; - rclcpp::Publisher::SharedPtr servo_set_pub_; - rclcpp::Subscription::SharedPtr servo_set_res_sub_; - rclcpp::Publisher::SharedPtr servo_move_pub_; - rclcpp::Subscription::SharedPtr servo_move_res_sub_; + rclcpp::Subscription::SharedPtr servo_get_sub_; + rclcpp::Publisher::SharedPtr servo_move_pub_; + rclcpp::Subscription::SharedPtr servo_move_res_sub_; - rclcpp::Subscription::SharedPtr relay_get_sub_; - rclcpp::Publisher::SharedPtr relay_move_pub_; - rclcpp::Subscription::SharedPtr relay_move_res_sub_; + rclcpp::Subscription::SharedPtr relay_get_sub_; + rclcpp::Publisher::SharedPtr relay_move_pub_; + rclcpp::Subscription::SharedPtr relay_move_res_sub_; }; } // namespace Modelec diff --git a/src/modelec_gui/include/modelec_gui/pages/action_page.new.hpp b/src/modelec_gui/include/modelec_gui/pages/action_page.new.hpp new file mode 100644 index 0000000..1d8361f --- /dev/null +++ b/src/modelec_gui/include/modelec_gui/pages/action_page.new.hpp @@ -0,0 +1,102 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include +#include + +namespace ModelecGUI +{ + class ActionPage : public QWidget + { + Q_OBJECT + + public: + + using ActionAscPos = modelec_interfaces::msg::ActionAscPos; + using ActionServoPosArray = modelec_interfaces::msg::ActionServoPosArray; + using ActionRelayStateArray = modelec_interfaces::msg::ActionRelayStateArray; + using ActionServoTimedArray = modelec_interfaces::msg::ActionServoTimedArray; + using ActionExec = modelec_interfaces::msg::ActionExec; + + ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr); + ~ActionPage() override; + + rclcpp::Node::SharedPtr get_node() const { return node_; } + + private: + + QVBoxLayout* main_layout_; + QPushButton* deploy_max_button_; + + // ---- Action1 front ---- + QHBoxLayout* deploy_action1_layout_; + QVBoxLayout* deploy_action1_front_layout_; + QLabel* deploy_action1_front_label_; + QPushButton* deploy_action1_front_up_button_; + + QHBoxLayout* deploy_action1_front_servos_layout_; + + QPushButton* deploy_action1_front_servo1_button_, + *deploy_action1_front_servo2_button_, + *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_; + + // ---- Action1 back ---- + QVBoxLayout* deploy_action1_back_layout_; + QLabel* deploy_action1_back_label_; + QPushButton* deploy_action1_back_up_button_; + + QHBoxLayout* deploy_action1_back_servos_layout_; + QPushButton* deploy_action1_back_servo1_button_, + *deploy_action1_back_servo2_button_, + *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_; + + // ---- Action2 ---- + QVBoxLayout* deploy_action2_layout_; + QLabel* deploy_action2_label_; + QPushButton* deploy_action2_button_; + QPushButton* undeploy_action2_button_; + + // ---- Ros ---- + rclcpp::Node::SharedPtr node_; + + rclcpp::Publisher::SharedPtr action_exec_pub_; + + rclcpp::Subscription::SharedPtr servo_get_sub_; + rclcpp::Publisher::SharedPtr servo_move_pub_; + rclcpp::Subscription::SharedPtr servo_move_res_sub_; + + rclcpp::Publisher::SharedPtr servo_timed_move_pub_; + rclcpp::Subscription::SharedPtr servo_timed_move_res_sub_; + + }; +} // namespace Modelec diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index a84aa16..35c5293 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -17,14 +17,14 @@ namespace ModelecGUI { //odo_page_ = new OdoPage(get_node(), this); test_map_page_ = new TestMapPage(get_node(), this); map_page_ = new MapPage(get_node(), this); - // action_page_ = new ActionPage(get_node(), this); + action_page_ = new ActionPage(get_node(), this); // alim_page_ = new AlimPage(get_node(), this); stackedWidget->addWidget(home_page_); // stackedWidget->addWidget(odo_page_); stackedWidget->addWidget(test_map_page_); stackedWidget->addWidget(map_page_); - // stackedWidget->addWidget(action_page_); + stackedWidget->addWidget(action_page_); // stackedWidget->addWidget(alim_page_); setCentralWidget(stackedWidget); diff --git a/src/modelec_gui/src/pages/action_page.cpp b/src/modelec_gui/src/pages/action_page.cpp index f28a5cd..9afe327 100644 --- a/src/modelec_gui/src/pages/action_page.cpp +++ b/src/modelec_gui/src/pages/action_page.cpp @@ -16,8 +16,8 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::DEPLOY_MAX_SIZE; - action_exec.mission = {ActionExec::ASC_GO_UP, ActionExec::RETRACT_BOTTOM_PLATE}; + // action_exec.action = ActionExec::DEPLOY_MAX; + // action_exec.mission = {ActionExec::DEPLOY_MAX_STEP}; action_exec_pub_->publish(action_exec); }); @@ -120,24 +120,42 @@ namespace ModelecGUI servo_actions_.push_back(servo6_action_); servo_actions_.push_back(servo7_action_); servo_actions_.push_back(servo8_action_); - waiting_for_move_servo_ = std::vector(servo_actions_.size(), false); for (size_t i = 0; i < servo_actions_.size(); ++i) { connect(servo_actions_[i], &ActionWidget::ButtonClicked, this, [this, i](double value) { - ActionServoPos servo_move; - servo_move.id = i; - servo_move.pos = 128; - servo_move.angle = value * M_PI / 180.0; - servo_set_pub_->publish(servo_move); - servo_actions_[i]->setDisabled(true); + ActionServoPosArray servo_move; + servo_move.items.resize(1); - waiting_for_move_servo_[i] = true; + servo_move.items[0].id = i; + servo_move.items[0].angle = value * M_PI / 180.0; + servo_move_pub_->publish(servo_move); + servo_actions_[i]->setDisabled(true); }); } + test_button_ = new QPushButton("Test POC Servos"); + connect(test_button_, &QPushButton::clicked, this, + [this]() + { + test_ = !test_; + + ActionServoPosArray servo_move; + servo_move.items.resize(2); + + servo_move.items[0].id = 1; + servo_move.items[0].angle = test_ ? M_PI_2 : 0; + servo_actions_[1]->setDisabled(true); + + servo_move.items[1].id = 2; + servo_move.items[1].angle = test_ ? 0 : M_PI_2; + servo_actions_[2]->setDisabled(true); + + servo_move_pub_->publish(servo_move); + }); + relay_layout_ = new QHBoxLayout; relay_top_button_ = new QPushButton(this); @@ -163,9 +181,10 @@ namespace ModelecGUI connect(relay_buttons_[i], &QPushButton::clicked, this, [this, i]() { - ActionRelayState relay_state; - relay_state.id = i; - relay_state.state = !relay_values_[i]; + ActionRelayStateArray relay_state; + relay_state.items.resize(1); + relay_state.items[0].id = i; + relay_state.items[0].state = !relay_values_[i]; relay_move_pub_->publish(relay_state); relay_buttons_[i]->setDisabled(true); }); @@ -176,14 +195,15 @@ namespace ModelecGUI [this]() { ActionExec action_exec; - action_exec.action = ActionExec::DEPLOY_BANNER; - action_exec.mission = {ActionExec::DEPLOY_BANNER_STEP}; + action_exec.action = ActionExec::DOWN; + action_exec.mission = {ActionExec::DOWN_STEP}; action_exec_pub_->publish(action_exec); }); layout_->addWidget(max_size_button_); layout_->addLayout(asc_layout_); layout_->addLayout(servo_layout_); + layout_->addWidget(test_button_); layout_->addLayout(relay_layout_); layout_->addWidget(deploy_banner_button_); @@ -220,62 +240,58 @@ namespace ModelecGUI asc_action_->setDisabled(false); }); - servo_get_sub_ = node_->create_subscription( - "action/get/servo", 10, [this](const ActionServoPos::SharedPtr msg) + servo_get_sub_ = node_->create_subscription( + "action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr msg) { - if (static_cast(servo_actions_.size()) > msg->id && servo_actions_[msg->id] != nullptr) + for (const auto& item : msg->items) { - servo_actions_[msg->id]->SetSpinBoxValue(msg->angle * 180.0 / M_PI); + if (static_cast(servo_actions_.size()) > item.id && servo_actions_[item.id] != nullptr) + { + servo_actions_[item.id]->SetSpinBoxValue(item.angle * 180.0 / M_PI); + } } }); - servo_set_pub_ = node_->create_publisher( - "action/set/servo", 10); - - servo_set_res_sub_ = node_->create_subscription( - "action/set/servo/res", 10, [this](const ActionServoPos::SharedPtr msg) - { - if (waiting_for_move_servo_[msg->id]) - { - ActionServoPos servo_move; - servo_move.id = msg->id; - servo_move.pos = msg->pos; - servo_move_pub_->publish(servo_move); - waiting_for_move_servo_[msg->id] = false; - } - }); - - servo_move_pub_ = node_->create_publisher( + servo_move_pub_ = node_->create_publisher( "action/move/servo", 10); - servo_move_res_sub_ = node_->create_subscription( - "action/move/servo/res", 10, [this](const ActionServoPos::SharedPtr msg) + servo_move_res_sub_ = node_->create_subscription( + "action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr msg) { - if (static_cast(servo_actions_.size()) > msg->id && servo_actions_[msg->id] != nullptr) + for (const auto& item : msg->items) { - servo_actions_[msg->id]->setDisabled(false); + if (static_cast(servo_actions_.size()) > item.id && servo_actions_[item.id] != nullptr) + { + servo_actions_[item.id]->setDisabled(false); + } } }); - relay_get_sub_ = node_->create_subscription( - "action/get/relay", 10, [this](const ActionRelayState::SharedPtr msg) + relay_get_sub_ = node_->create_subscription( + "action/get/relay", 10, [this](const ActionRelayStateArray::SharedPtr msg) { - if (static_cast(relay_values_.size()) > msg->id) + for (const auto& item : msg->items) { - relay_values_[msg->id] = msg->state; + if (static_cast(relay_buttons_.size()) > item.id && relay_buttons_[item.id] != nullptr) + { + relay_values_[item.id] = item.state; + } } }); - relay_move_pub_ = node_->create_publisher( + relay_move_pub_ = node_->create_publisher( "action/move/relay", 10); - relay_move_res_sub_ = node_->create_subscription( - "action/move/relay/res", 10, [this](const ActionRelayState::SharedPtr msg) + relay_move_res_sub_ = node_->create_subscription( + "action/move/relay/res", 10, [this](const ActionRelayStateArray::SharedPtr msg) { - if (static_cast(relay_buttons_.size()) > msg->id && relay_buttons_[msg->id] != nullptr && static_cast(relay_values_.size()) > msg->id) + for (const auto& item : msg->items) { - relay_buttons_[msg->id]->setDisabled(false); - relay_values_[msg->id] = msg->state; + if (static_cast(relay_buttons_.size()) > item.id && relay_buttons_[item.id] != nullptr) + { + relay_buttons_[item.id]->setDisabled(false); + relay_values_[item.id] = item.state; + } } }); } diff --git a/src/modelec_gui/src/pages/action_page.new.cpp b/src/modelec_gui/src/pages/action_page.new.cpp new file mode 100644 index 0000000..f875d79 --- /dev/null +++ b/src/modelec_gui/src/pages/action_page.new.cpp @@ -0,0 +1,295 @@ +#include + +namespace ModelecGUI +{ + ActionPage::ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent) : + QWidget(parent), + node_(node) + { + action_exec_pub_ = node_->create_publisher( + "action/exec", 10); + + servo_get_sub_ = node_->create_subscription( + "action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr) + { + }); + + servo_move_pub_ = node_->create_publisher( + "action/move/servo", 10); + + servo_move_res_sub_ = node_->create_subscription( + "action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr) + { + }); + + servo_timed_move_pub_ = node_->create_publisher( + "action/move/servo/timed", 10); + + servo_timed_move_res_sub_ = node_->create_subscription( + "action/move/servo/timed/res", 10, [this](const ActionServoTimedArray::SharedPtr) + { + }); + + main_layout_ = new QVBoxLayout(this); + setLayout(main_layout_); + setWindowTitle("Action Page"); + + deploy_max_button_ = new QPushButton("Deploy Max Size"); + connect(deploy_max_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::MAX_DEPLOY; + action_exec.mission = {ActionExec::MAX_DEPLOY_STEP}; + action_exec_pub_->publish(action_exec); + }); + + deploy_action1_layout_ = new QHBoxLayout; + deploy_action1_front_layout_ = new QVBoxLayout; + deploy_action1_front_label_ = new QLabel("Front Action 1"); + + deploy_action1_front_up_button_ = new QPushButton("Up"); + deploy_action1_front_down_button_ = new QPushButton("Down"); + + connect(deploy_action1_front_up_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::FRONT_UP; + action_exec.mission = {ActionExec::FRONT_UP_STEP}; + action_exec_pub_->publish(action_exec); + }); + + connect(deploy_action1_front_down_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::FRONT_DOWN; + action_exec.mission = {ActionExec::FRONT_DOWN_STEP}; + action_exec_pub_->publish(action_exec); + }); + + deploy_action1_front_servos_layout_ = new QHBoxLayout; + + deploy_action1_front_servo1_button_ = new QPushButton("Servo 1"); + deploy_action1_front_servo2_button_ = new QPushButton("Servo 2"); + deploy_action1_front_servo3_button_ = new QPushButton("Servo 3"); + deploy_action1_front_servo4_button_ = new QPushButton("Servo 4"); + + 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::FRONT_TAKE_1 : ActionExec::FRONT_FREE_1; + + action_exec.mission = {deploy_action1_front_servo1_state_ + ? ActionExec::FRONT_TAKE_1_STEP : ActionExec::FRONT_FREE_1_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + 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::FRONT_TAKE_2 : ActionExec::FRONT_FREE_2; + + action_exec.mission = {deploy_action1_front_servo2_state_ + ? ActionExec::FRONT_TAKE_2_STEP : ActionExec::FRONT_FREE_2_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + 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::FRONT_TAKE_3 : ActionExec::FRONT_FREE_3; + + action_exec.mission = {deploy_action1_front_servo3_state_ + ? ActionExec::FRONT_TAKE_3_STEP : ActionExec::FRONT_FREE_3_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + 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::FRONT_TAKE_4 : ActionExec::FRONT_FREE_4; + + action_exec.mission = {deploy_action1_front_servo4_state_ + ? ActionExec::FRONT_TAKE_4_STEP : ActionExec::FRONT_FREE_4_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo1_button_); + deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo2_button_); + deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo3_button_); + deploy_action1_front_servos_layout_->addWidget(deploy_action1_front_servo4_button_); + + deploy_action1_front_layout_->addWidget(deploy_action1_front_label_); + 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_back_layout_ = new QVBoxLayout; + deploy_action1_back_label_ = new QLabel("Back Action 1"); + + deploy_action1_back_up_button_ = new QPushButton("Up"); + deploy_action1_back_down_button_ = new QPushButton("Down"); + + connect(deploy_action1_back_up_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::BACK_UP; + action_exec.mission = {ActionExec::BACK_UP_STEP}; + action_exec_pub_->publish(action_exec); + }); + + connect(deploy_action1_back_down_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::BACK_DOWN; + action_exec.mission = {ActionExec::BACK_DOWN_STEP}; + action_exec_pub_->publish(action_exec); + }); + + deploy_action1_back_servos_layout_ = new QHBoxLayout; + + deploy_action1_back_servo1_button_ = new QPushButton("Servo 1"); + deploy_action1_back_servo2_button_ = new QPushButton("Servo 2"); + deploy_action1_back_servo3_button_ = new QPushButton("Servo 3"); + deploy_action1_back_servo4_button_ = new QPushButton("Servo 4"); + + 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::BACK_TAKE_1 : ActionExec::BACK_FREE_1; + + action_exec.mission = {deploy_action1_back_servo1_state_ + ? ActionExec::BACK_TAKE_1_STEP : ActionExec::BACK_FREE_1_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + 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::BACK_TAKE_2 : ActionExec::BACK_FREE_2; + + action_exec.mission = {deploy_action1_back_servo2_state_ + ? ActionExec::BACK_TAKE_2_STEP : ActionExec::BACK_FREE_2_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + 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::BACK_TAKE_3 : ActionExec::BACK_FREE_3; + + action_exec.mission = {deploy_action1_back_servo3_state_ + ? ActionExec::BACK_TAKE_3_STEP : ActionExec::BACK_FREE_3_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + 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::BACK_TAKE_4 : ActionExec::BACK_FREE_4; + + action_exec.mission = {deploy_action1_back_servo4_state_ + ? ActionExec::BACK_TAKE_4_STEP : ActionExec::BACK_FREE_4_STEP}; + + action_exec_pub_->publish(action_exec); + }); + + deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo1_button_); + deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo2_button_); + deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo3_button_); + deploy_action1_back_servos_layout_->addWidget(deploy_action1_back_servo4_button_); + + deploy_action1_back_layout_->addWidget(deploy_action1_back_label_); + 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_layout_->addLayout(deploy_action1_front_layout_); + deploy_action1_layout_->addLayout(deploy_action1_back_layout_); + + deploy_action2_layout_ = new QVBoxLayout; + deploy_action2_label_ = new QLabel("Thermometrer"); + deploy_action2_button_ = new QPushButton("Deploy"); + undeploy_action2_button_ = new QPushButton("Undeploy"); + + connect(deploy_action2_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::THERMO_DEPLOY; + action_exec.mission = {ActionExec::THERMO_DEPLOY_STEP}; + action_exec_pub_->publish(action_exec); + }); + + connect(undeploy_action2_button_, &QPushButton::clicked, this, + [this]() + { + ActionExec action_exec; + action_exec.action = ActionExec::THERMO_UNDEPLOY; + action_exec.mission = {ActionExec::THERMO_UNDEPLOY_STEP}; + action_exec_pub_->publish(action_exec); + }); + + deploy_action2_layout_->addWidget(deploy_action2_label_); + deploy_action2_layout_->addWidget(deploy_action2_button_); + deploy_action2_layout_->addWidget(undeploy_action2_button_); + + main_layout_->addWidget(deploy_max_button_); + main_layout_->addLayout(deploy_action1_layout_); + main_layout_->addLayout(deploy_action2_layout_); + } + + ActionPage::~ActionPage() + = default; +} diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt index cf23ddb..154211c 100644 --- a/src/modelec_interfaces/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/Alim/AlimEmg.msg" + "msg/Odometry/OdometryPos.msg" "msg/Odometry/OdometryGoTo.msg" "msg/Odometry/OdometrySpeed.msg" @@ -22,19 +23,28 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/Odometry/OdometryWaypoints.msg" "msg/Odometry/OdometryStart.msg" "msg/Odometry/PID/OdometryPid.msg" + "msg/Strat/StratState.msg" + "msg/Map/Map.msg" "msg/Map/Obstacle.msg" "msg/Map/Spawn.msg" + "msg/Action/ActionAscPos.msg" "msg/Action/ActionRelayState.msg" "msg/Action/ActionServoPos.msg" + "msg/Action/ActionServoTimed.msg" "msg/Action/ActionExec.msg" + "msg/Action/Array/ActionServoPosArray.msg" + "msg/Action/Array/ActionServoTimedArray.msg" + "msg/Action/Array/ActionRelayStateArray.msg" + "srv/Alim/AlimOut.srv" "srv/Alim/AlimIn.srv" "srv/Alim/AlimTemp.srv" "srv/Alim/AlimBau.srv" "srv/Alim/AlimEmg.srv" + "srv/Odometry/OdometryPosition.srv" "srv/Odometry/OdometrySpeed.srv" "srv/Odometry/OdometryToF.srv" @@ -42,8 +52,10 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/Odometry/OdometryAddWaypoint.srv" "srv/Odometry/PID/OdometryGetPid.srv" "srv/Odometry/PID/OdometrySetPid.srv" + "srv/Map/Map.srv" "srv/Map/MapSize.srv" + "srv/AddServoMotor.srv" "srv/AddSolenoid.srv" "srv/Tirette.srv" diff --git a/src/modelec_interfaces/msg/Action/ActionExec.msg b/src/modelec_interfaces/msg/Action/ActionExec.msg index 7943836..2ccf1bd 100644 --- a/src/modelec_interfaces/msg/Action/ActionExec.msg +++ b/src/modelec_interfaces/msg/Action/ActionExec.msg @@ -1,28 +1,64 @@ +# Action int32 NONE = 0 -int32 DEPLOY_BANNER = 1 -int32 UNDEPLOY_BANNER = 2 -int32 TAKE_POT = 3 -int32 PLACE_POT = 4 -int32 DEPLOY_MAX_SIZE = 5 +int32 FRONT_UP = 1 +int32 FRONT_DOWN = 2 -int32 DEPLOY_BANNER_STEP = 0 -int32 UNDEPLOY_BANNER_STEP = 1 +int32 FRONT_TAKE_1 = 10 +int32 FRONT_FREE_1 = 11 +int32 FRONT_TAKE_2 = 20 +int32 FRONT_FREE_2 = 21 +int32 FRONT_TAKE_3 = 30 +int32 FRONT_FREE_3 = 31 +int32 FRONT_TAKE_4 = 40 +int32 FRONT_FREE_4 = 41 -int32 ASC_GO_DOWN = 2 -int32 STICK_TO_STRUCT = 3 -int32 ASC_GO_UP = 4 -int32 RETRACT_BOTTOM_PLATE = 5 -int32 ASC_GO_DOWN_TO_TAKE_POT = 6 -int32 STICK_POT = 7 -int32 ASC_GO_UP_TO_TAKE_POT = 8 -int32 PLACE_FIRST_PLATE = 9 +int32 BACK_UP = 101 +int32 BACK_DOWN = 102 -int32 STICK_ALL = 10 +int32 BACK_TAKE_1 = 110 +int32 BACK_FREE_1 = 111 +int32 BACK_TAKE_2 = 120 +int32 BACK_FREE_2 = 121 +int32 BACK_TAKE_3 = 130 +int32 BACK_FREE_3 = 131 +int32 BACK_TAKE_4 = 140 +int32 BACK_FREE_4 = 141 -int32 ASC_FINAL = 11 -int32 FREE_ALL = 12 -int32 REMOVE_ACTION_STEP = 13 +int32 THERMO_DEPLOY = 200 +int32 THERMO_UNDEPLOY = 201 +int32 MAX_DEPLOY = 99 + + +# Step +int32 FRONT_UP_STEP = 1 +int32 FRONT_DOWN_STEP = 2 + +int32 FRONT_TAKE_1_STEP = 10 +int32 FRONT_FREE_1_STEP = 11 +int32 FRONT_TAKE_2_STEP = 20 +int32 FRONT_FREE_2_STEP = 21 +int32 FRONT_TAKE_3_STEP = 30 +int32 FRONT_FREE_3_STEP = 31 +int32 FRONT_TAKE_4_STEP = 40 +int32 FRONT_FREE_4_STEP = 41 + +int32 BACK_UP_STEP = 101 +int32 BACK_DOWN_STEP = 102 + +int32 BACK_TAKE_1_STEP = 110 +int32 BACK_FREE_1_STEP = 111 +int32 BACK_TAKE_2_STEP = 120 +int32 BACK_FREE_2_STEP = 121 +int32 BACK_TAKE_3_STEP = 130 +int32 BACK_FREE_3_STEP = 131 +int32 BACK_TAKE_4_STEP = 140 +int32 BACK_FREE_4_STEP = 141 + +int32 THERMO_DEPLOY_STEP = 200 +int32 THERMO_UNDEPLOY_STEP = 201 + +int32 MAX_DEPLOY_STEP = 99 int32[] mission int32 action diff --git a/src/modelec_interfaces/msg/Action/ActionServoPos.msg b/src/modelec_interfaces/msg/Action/ActionServoPos.msg index 5a45d99..8f6ea56 100644 --- a/src/modelec_interfaces/msg/Action/ActionServoPos.msg +++ b/src/modelec_interfaces/msg/Action/ActionServoPos.msg @@ -1,4 +1,3 @@ int32 id -int32 pos float64 angle bool success \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Action/ActionServoTimed.msg b/src/modelec_interfaces/msg/Action/ActionServoTimed.msg new file mode 100644 index 0000000..b54ca83 --- /dev/null +++ b/src/modelec_interfaces/msg/Action/ActionServoTimed.msg @@ -0,0 +1,5 @@ +int32 id +float64 start_angle +float64 end_angle +float64 duration_s +bool success \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Action/Array/ActionRelayStateArray.msg b/src/modelec_interfaces/msg/Action/Array/ActionRelayStateArray.msg new file mode 100644 index 0000000..cf3e07e --- /dev/null +++ b/src/modelec_interfaces/msg/Action/Array/ActionRelayStateArray.msg @@ -0,0 +1,2 @@ +ActionRelayState[] items +bool success \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Action/Array/ActionServoPosArray.msg b/src/modelec_interfaces/msg/Action/Array/ActionServoPosArray.msg new file mode 100644 index 0000000..7470fec --- /dev/null +++ b/src/modelec_interfaces/msg/Action/Array/ActionServoPosArray.msg @@ -0,0 +1,2 @@ +ActionServoPos[] items +bool success \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Action/Array/ActionServoTimedArray.msg b/src/modelec_interfaces/msg/Action/Array/ActionServoTimedArray.msg new file mode 100644 index 0000000..b744f5f --- /dev/null +++ b/src/modelec_interfaces/msg/Action/Array/ActionServoTimedArray.msg @@ -0,0 +1,2 @@ +ActionServoTimed[] items +bool success \ No newline at end of file diff --git a/src/modelec_strat/CMakeLists.txt b/src/modelec_strat/CMakeLists.txt index 7ad1a6e..4a52189 100644 --- a/src/modelec_strat/CMakeLists.txt +++ b/src/modelec_strat/CMakeLists.txt @@ -15,7 +15,7 @@ find_package(ament_index_cpp REQUIRED) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/go_home_mission.cpp src/missions/take_send_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp src/deposite_zone.cpp) +add_executable(strat_fsm src/strat_fms.cpp src/navigation_helper.cpp src/pathfinding.cpp src/action_executor.cpp src/missions/go_home_mission.cpp src/missions/take_mission.cpp src/missions/free_mission.cpp src/obstacle/obstacle.cpp src/obstacle/box.cpp src/deposite_zone.cpp) ament_target_dependencies(strat_fsm rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp) target_link_libraries(strat_fsm modelec_utils::utils modelec_utils::config) target_include_directories(strat_fsm PUBLIC diff --git a/src/modelec_strat/data/deposite_zone.xml b/src/modelec_strat/data/deposite_zone.xml index f76152f..99fd522 100644 --- a/src/modelec_strat/data/deposite_zone.xml +++ b/src/modelec_strat/data/deposite_zone.xml @@ -1,32 +1,32 @@ - + - + - + - + - + - + - + - + - + - + diff --git a/src/modelec_strat/data/obstacles.xml b/src/modelec_strat/data/obstacles.xml index cac6d2c..0d46ad6 100644 --- a/src/modelec_strat/data/obstacles.xml +++ b/src/modelec_strat/data/obstacles.xml @@ -1,27 +1,27 @@ - - + + + - + - + - + + diff --git a/src/modelec_strat/include/modelec_strat/action_executor.hpp b/src/modelec_strat/include/modelec_strat/action_executor.hpp index 3430b34..431eecd 100644 --- a/src/modelec_strat/include/modelec_strat/action_executor.hpp +++ b/src/modelec_strat/include/modelec_strat/action_executor.hpp @@ -2,8 +2,9 @@ #include #include -#include -#include +#include +#include +#include #include namespace Modelec @@ -13,15 +14,68 @@ namespace Modelec public: enum Action { - NONE, - TAKE, - SEND, + NONE = 0, + + FRONT_UP = 1, + FRONT_DOWN = 2, + + FRONT_TAKE_1 = 10, + FRONT_FREE_1 = 11, + FRONT_TAKE_2 = 20, + FRONT_FREE_2 = 21, + FRONT_TAKE_3 = 30, + FRONT_FREE_3 = 31, + FRONT_TAKE_4 = 40, + FRONT_FREE_4 = 41, + + BACK_UP = 101, + BACK_DOWN = 102, + + BACK_TAKE_1 = 110, + BACK_FREE_1 = 111, + BACK_TAKE_2 = 120, + BACK_FREE_2 = 121, + BACK_TAKE_3 = 130, + BACK_FREE_3 = 131, + BACK_TAKE_4 = 140, + BACK_FREE_4 = 141, + + THERMO_DEPLOY = 200, + THERMO_UNDEPLOY = 201, + + MAX_DEPLOY = 99 }; enum Step { - TAKE_STEP, - SEND_STEP + FRONT_UP_STEP = 1, + FRONT_DOWN_STEP = 2, + + FRONT_TAKE_1_STEP = 10, + FRONT_FREE_1_STEP = 11, + FRONT_TAKE_2_STEP = 20, + FRONT_FREE_2_STEP = 21, + FRONT_TAKE_3_STEP = 30, + FRONT_FREE_3_STEP = 31, + FRONT_TAKE_4_STEP = 40, + FRONT_FREE_4_STEP = 41, + + BACK_UP_STEP = 101, + BACK_DOWN_STEP = 102, + + BACK_TAKE_1_STEP = 110, + BACK_FREE_1_STEP = 111, + BACK_TAKE_2_STEP = 120, + BACK_FREE_2_STEP = 121, + BACK_TAKE_3_STEP = 130, + BACK_FREE_3_STEP = 131, + BACK_TAKE_4_STEP = 140, + BACK_FREE_4_STEP = 141, + + THERMO_DEPLOY_STEP = 200, + THERMO_UNDEPLOY_STEP = 201, + + MAX_DEPLOY_STEP = 99, }; ActionExecutor(); @@ -36,18 +90,24 @@ namespace Modelec void ReInit(); - void Send(); + void Down(bool front = true); - void Take(); + void Up(bool front = true); + + void Take(bool front = true, int n = 1); + + void Free(bool front = true, int n = 1); protected: rclcpp::Publisher::SharedPtr asc_move_pub_; - rclcpp::Publisher::SharedPtr servo_move_pub_; - rclcpp::Publisher::SharedPtr relay_move_pub_; + rclcpp::Publisher::SharedPtr servo_move_pub_; + rclcpp::Publisher::SharedPtr relay_move_pub_; + rclcpp::Publisher::SharedPtr servo_timed_move_pub_; rclcpp::Subscription::SharedPtr asc_move_res_sub_; - rclcpp::Subscription::SharedPtr servo_move_res_sub_; - rclcpp::Subscription::SharedPtr relay_move_res_sub_; + rclcpp::Subscription::SharedPtr servo_move_res_sub_; + rclcpp::Subscription::SharedPtr relay_move_res_sub_; + rclcpp::Subscription::SharedPtr servo_timed_move_res_sub_; rclcpp::Subscription::SharedPtr action_exec_sub_; @@ -58,6 +118,14 @@ namespace Modelec bool action_done_ = true; int step_running_ = 0; + bool servo_step_by_step_; + + int step_value_; + + int goal_value_; + + int current_step_; + private: rclcpp::Node::SharedPtr node_; }; diff --git a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp index f3423e6..5179646 100644 --- a/src/modelec_strat/include/modelec_strat/deposite_zone.hpp +++ b/src/modelec_strat/include/modelec_strat/deposite_zone.hpp @@ -12,41 +12,23 @@ namespace Modelec public: DepositeZone(tinyxml2::XMLElement* obstacleElem); - Point GetPosition() const { return position_; } + Point GetPosition() const; - Point GetNextPotPos() - { - if (pot_queue_.empty()) - return Point(std::numeric_limits::min(), std::numeric_limits::min(), 0); - return pot_queue_.front(); - } + void Validate(bool valid); - Point ValidNextPotPos() - { - if (pot_queue_.empty()) - return Point(std::numeric_limits::min(), std::numeric_limits::min(), 0); - Point p = pot_queue_.front(); - pot_queue_.pop(); - return p; - } + bool Validate() const; - int GetTeam() const { return team_; } - int GetId() const { return id_; } - int GetMaxPot() const { return max_pot_; } + int GetId() const; + int GetMaxPot() const; - int GetWidth() const { return w_; } - int GetHeight() const { return h_; } - - int RemainingPotPos() const - { - return pot_queue_.size(); - } + int GetWidth() const; + int GetHeight() const; protected: - int id_, team_, max_pot_; + int id_, max_pot_; int w_, h_; Point position_; - std::queue pot_queue_; + bool has_box_ = false; }; } diff --git a/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp new file mode 100644 index 0000000..d56a4a3 --- /dev/null +++ b/src/modelec_strat/include/modelec_strat/missions/free_mission.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include +#include +#include + +namespace Modelec { + class FreeMission : public Mission { + public: + FreeMission(const std::shared_ptr& nav, + const std::shared_ptr& action_executor); + + void Start(rclcpp::Node::SharedPtr node) override; + void Update() override; + void Clear() override; + MissionStatus GetStatus() const override; + std::string GetName() const override { return "Free"; } + + private: + enum Step + { + GO_TO_FREE, + FREE, + DONE, + } step_; + + MissionStatus status_; + std::shared_ptr nav_; + std::shared_ptr action_executor_; + rclcpp::Node::SharedPtr node_; + + std::shared_ptr target_deposite_zone_; + + rclcpp::Time go_timeout_; + rclcpp::Time deploy_time_; + }; +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp similarity index 74% rename from src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp rename to src/modelec_strat/include/modelec_strat/missions/take_mission.hpp index c023f35..d194339 100644 --- a/src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp +++ b/src/modelec_strat/include/modelec_strat/missions/take_mission.hpp @@ -5,32 +5,28 @@ #include namespace Modelec { - class TakeSendMission : public Mission { + class TakeMission : public Mission { public: - TakeSendMission(const std::shared_ptr& nav, + TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor); void Start(rclcpp::Node::SharedPtr node) override; void Update() override; void Clear() override; MissionStatus GetStatus() const override; - std::string GetName() const override { return "GoHome"; } + std::string GetName() const override { return "Take"; } private: enum Step { GO_TO_TAKE, TAKE, - WAIT_5S, - GO_TO_SEND, - SEND, DONE, } step_; MissionStatus status_; std::shared_ptr nav_; std::shared_ptr action_executor_; - rclcpp::Time go_home_time_; rclcpp::Node::SharedPtr node_; rclcpp::Time go_timeout_; diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index c2b71fc..1932dab 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -87,8 +87,12 @@ namespace Modelec bool LoadDepositeZoneFromXML(const std::string& filename); - std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, - const std::vector& blacklistedId = {}); + std::shared_ptr GetClosestDepositeZone(const PosMsg::SharedPtr& pos, + const std::vector& blacklistedId = {}, bool only_free = false); + + template ::value>> + std::shared_ptr GetClosestObstacle(const PosMsg::SharedPtr& pos) const; PosMsg::SharedPtr GetHomePosition(); @@ -171,4 +175,27 @@ namespace Modelec rclcpp::Time last_odo_get_pos_time_; }; + + template + std::shared_ptr NavigationHelper::GetClosestObstacle(const PosMsg::SharedPtr& pos) const + { + std::shared_ptr closest_obstacle = nullptr; + auto robotPos = Point(pos->x, pos->y, pos->theta); + float distance = std::numeric_limits::max(); + + for (const auto& obstacle : GetPathfinding()->GetObstacles()) + { + if (auto obs = std::dynamic_pointer_cast(obstacle.second)) + { + auto dist = Point::distance(robotPos, obs->GetPosition()); + if (dist < distance) + { + distance = dist; + closest_obstacle = obs; + } + } + } + + return closest_obstacle; + } } \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index 5ea5d5a..fe15f90 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -79,6 +79,10 @@ namespace Modelec [[nodiscard]] std::shared_ptr GetObstacle(int id) const; + std::map> GetObstacles() const { + return obstacle_map_; + } + void RemoveObstacle(int id); void AddObstacle(const std::shared_ptr& obstacle); diff --git a/src/modelec_strat/include/modelec_strat/strat_fms.hpp b/src/modelec_strat/include/modelec_strat/strat_fms.hpp index 0a5ccc9..d552887 100644 --- a/src/modelec_strat/include/modelec_strat/strat_fms.hpp +++ b/src/modelec_strat/include/modelec_strat/strat_fms.hpp @@ -24,8 +24,10 @@ namespace Modelec INIT, WAIT_START, SELECT_MISSION, + SELECT_GAME_ACTION, - TAKE_SEND_MISSION, + TAKE_MISSION, + FREE_MISSION, DO_GO_HOME, STOP @@ -59,6 +61,8 @@ namespace Modelec std::unique_ptr current_mission_; int team_id_ = 0; + std::queue game_action_sequence_; + std::shared_ptr nav_; std::shared_ptr action_executor_; diff --git a/src/modelec_strat/src/action_executor.cpp b/src/modelec_strat/src/action_executor.cpp index 6247e3a..ede96da 100644 --- a/src/modelec_strat/src/action_executor.cpp +++ b/src/modelec_strat/src/action_executor.cpp @@ -9,8 +9,8 @@ namespace Modelec ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node) { asc_move_pub_ = node_->create_publisher("/action/move/asc", 10); - servo_move_pub_ = node_->create_publisher("/action/move/servo", 10); - relay_move_pub_ = node_->create_publisher("/action/move/relay", 10); + servo_move_pub_ = node_->create_publisher("/action/move/servo", 10); + relay_move_pub_ = node_->create_publisher("/action/move/relay", 10); asc_move_res_sub_ = node_->create_subscription( "/action/move/asc/res", 10, [this](const modelec_interfaces::msg::ActionAscPos::SharedPtr) @@ -19,17 +19,17 @@ namespace Modelec Update(); }); - servo_move_res_sub_ = node_->create_subscription( - "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr) + servo_move_res_sub_ = node_->create_subscription( + "/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPosArray::SharedPtr msg) { - step_running_--; + step_running_ -= msg->items.size(); Update(); }); - relay_move_res_sub_ = node_->create_subscription( - "/action/move/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr) + relay_move_res_sub_ = node_->create_subscription( + "/action/move/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayStateArray::SharedPtr msg) { - step_running_--; + step_running_ -= msg->items.size(); Update(); }); @@ -45,6 +45,15 @@ namespace Modelec step_running_ = 0; Update(); }); + + servo_timed_move_pub_ = node_->create_publisher("/action/move/servo/timed", 10); + + servo_timed_move_res_sub_ = node_->create_subscription( + "/action/move/servo/timed/res", 10, [this](const modelec_interfaces::msg::ActionServoTimedArray::SharedPtr msg) + { + step_running_ -= msg->items.size(); + Update(); + }); } rclcpp::Node::SharedPtr ActionExecutor::GetNode() const @@ -70,24 +79,98 @@ namespace Modelec { switch (step_.front()) { - case SEND_STEP: + case FRONT_DOWN_STEP: { - modelec_interfaces::msg::ActionServoPos msg; - msg.id = 5; - msg.pos = 1; - servo_move_pub_->publish(msg); + modelec_interfaces::msg::ActionServoTimedArray msg; + msg.items.resize(4); - step_running_ = 1; + msg.items[0].id = 0; + msg.items[0].start_angle = 1.49; + msg.items[0].end_angle = 0; + msg.items[0].duration_s = 1; + + msg.items[1].id = 1; + msg.items[1].start_angle = 1.5; + msg.items[1].end_angle = 3; + msg.items[1].duration_s = 1; + + msg.items[2].id = 4; + msg.items[2].start_angle = 3; + msg.items[2].end_angle = 1.57; + msg.items[2].duration_s = 1; + + msg.items[3].id = 5; + msg.items[3].start_angle = 0; + msg.items[3].end_angle = 1.4; + msg.items[3].duration_s = 1; + + servo_timed_move_pub_->publish(msg); + + step_running_ = msg.items.size(); } break; - case TAKE_STEP: + case FRONT_UP_STEP: { - modelec_interfaces::msg::ActionServoPos msg; - msg.id = 6; - msg.pos = 1; + modelec_interfaces::msg::ActionServoTimedArray msg; + msg.items.resize(4); + + msg.items[0].id = 0; + msg.items[0].start_angle = 0; + msg.items[0].end_angle = 1.49; + msg.items[0].duration_s = 1; + + msg.items[1].id = 1; + msg.items[1].start_angle = 3; + msg.items[1].end_angle = 1.5; + msg.items[1].duration_s = 1; + + msg.items[2].id = 4; + msg.items[2].start_angle = 1.57; + msg.items[2].end_angle = 3; + msg.items[2].duration_s = 1; + + msg.items[3].id = 5; + msg.items[3].start_angle = 1.4; + msg.items[3].end_angle = 0; + msg.items[3].duration_s = 1; + + servo_timed_move_pub_->publish(msg); + + step_running_ = msg.items.size(); + } + break; + case FRONT_TAKE_1_STEP: + { + modelec_interfaces::msg::ActionServoPosArray msg; + + msg.items.resize(1); + + msg.items[0].id = 2; + msg.items[0].angle = 0; + servo_move_pub_->publish(msg); - step_running_ = 1; + step_running_ = msg.items.size(); + } + break; + case FRONT_FREE_1_STEP: + { + modelec_interfaces::msg::ActionServoPosArray msg; + + msg.items.resize(1); + + msg.items[0].id = 2; + msg.items[0].angle = 3; + + servo_move_pub_->publish(msg); + + step_running_ = msg.items.size(); + } + + break; + case MAX_DEPLOY_STEP: + { + } break; default: @@ -109,27 +192,53 @@ namespace Modelec action_ = NONE; } - void ActionExecutor::Send() { + void ActionExecutor::Down(bool front) { if (action_done_) { - action_ = SEND; + action_ = front ? FRONT_DOWN : BACK_DOWN; action_done_ = false; step_running_ = 0; - step_.push(SEND_STEP); + step_.push(front ? FRONT_DOWN_STEP : BACK_DOWN_STEP); Update(); } } - void ActionExecutor::Take() { + void ActionExecutor::Up(bool front) { if (action_done_) { - action_ = TAKE; + action_ = front ? FRONT_UP : BACK_UP; action_done_ = false; step_running_ = 0; - step_.push(TAKE_STEP); + step_.push(front ? FRONT_UP_STEP : BACK_UP_STEP); + + Update(); + } + } + + void ActionExecutor::Take(bool front, int n) { + if (action_done_) + { + action_ = static_cast(n * 10 + (front ? 0 : 100)); + action_done_ = false; + step_running_ = 0; + + step_.push(static_cast(n * 10 + (front ? 0 : 100))); + + Update(); + } + } + + void ActionExecutor::Free(bool front, int n) { + if (action_done_) + { + action_ = static_cast(1 + n * 10 + (front ? 0 : 100)); + action_done_ = false; + step_running_ = 0; + + step_.push(static_cast(1 + n * 10 + (front ? 0 : 100))); Update(); } diff --git a/src/modelec_strat/src/deposite_zone.cpp b/src/modelec_strat/src/deposite_zone.cpp index 27be957..64159ec 100644 --- a/src/modelec_strat/src/deposite_zone.cpp +++ b/src/modelec_strat/src/deposite_zone.cpp @@ -6,8 +6,6 @@ namespace Modelec DepositeZone::DepositeZone(tinyxml2::XMLElement* obstacleElem) { obstacleElem->QueryIntAttribute("id", &id_); - obstacleElem->QueryIntAttribute("team", &team_); - obstacleElem->QueryIntAttribute("max_pot", &max_pot_); auto posElem = obstacleElem->FirstChildElement("Pos"); if (posElem) @@ -18,19 +16,30 @@ namespace Modelec posElem->QueryIntAttribute("w", &w_); posElem->QueryIntAttribute("h", &h_); } - - auto posPotList = obstacleElem->FirstChildElement("PotPos"); - if (posPotList) - { - for (auto elemPos = posPotList->FirstChildElement("Pos"); elemPos; elemPos = elemPos-> - NextSiblingElement("Pos")) - { - Point pos; - elemPos->QueryIntAttribute("x", &pos.x); - elemPos->QueryIntAttribute("y", &pos.y); - elemPos->QueryDoubleAttribute("theta", &pos.theta); - pot_queue_.emplace(pos); - } - } } + + Point DepositeZone::GetPosition() const + { + return position_; + } + + void DepositeZone::Validate(bool valid) + { + has_box_ = valid; + } + + bool DepositeZone::Validate() const + { return has_box_; } + + int DepositeZone::GetId() const + { return id_; } + + int DepositeZone::GetMaxPot() const + { return max_pot_; } + + int DepositeZone::GetWidth() const + { return w_; } + + int DepositeZone::GetHeight() const + { return h_; } } diff --git a/src/modelec_strat/src/missions/free_mission.cpp b/src/modelec_strat/src/missions/free_mission.cpp new file mode 100644 index 0000000..ac1d60e --- /dev/null +++ b/src/modelec_strat/src/missions/free_mission.cpp @@ -0,0 +1,86 @@ +#include + +namespace Modelec { + FreeMission::FreeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) + : step_(GO_TO_FREE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + } + + void FreeMission::Start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + go_timeout_ = node_->now(); + + status_ = MissionStatus::RUNNING; + step_ = GO_TO_FREE; + } + + void FreeMission::Update() + { + if (!action_executor_->IsActionDone()) + { + return; + } + + if (!nav_->HasArrived()) + { + if ((node_->now() - go_timeout_).seconds() < 10) + { + return; + } + } + + switch (step_) + { + case GO_TO_FREE: + { + auto currPos = nav_->GetCurrentPos(); + + auto dist = std::clamp(Point::distance(Point(currPos->x, currPos->y, currPos->theta), + nav_->GetClosestDepositeZone(nav_->GetCurrentPos())->GetPosition()), 0.0, 200.0); + + target_deposite_zone_ = nav_->GetClosestDepositeZone(nav_->GetCurrentPos(), {}, true); + + auto depoPoint = target_deposite_zone_->GetPosition(); + + auto angle = atan2(depoPoint.y - currPos->y, + depoPoint.x - currPos->x); + + nav_->GoToRotateFirst(depoPoint.GetTakePosition(dist, + angle + M_PI), true, Pathfinding::FREE); + + go_timeout_ = node_->now(); + + step_ = FREE; + } + break; + case FREE: + { + action_executor_->Down(); + deploy_time_ = node_->now(); + + step_ = DONE; + } + break; + case DONE: + { + target_deposite_zone_->Validate(true); + } + + status_ = MissionStatus::DONE; + break; + default: + break; + } + } + + void FreeMission::Clear() + { + } + + MissionStatus FreeMission::GetStatus() const + { + return status_; + } + +} \ No newline at end of file diff --git a/src/modelec_strat/src/missions/go_home_mission.cpp b/src/modelec_strat/src/missions/go_home_mission.cpp index 754de11..7693465 100644 --- a/src/modelec_strat/src/missions/go_home_mission.cpp +++ b/src/modelec_strat/src/missions/go_home_mission.cpp @@ -17,21 +17,10 @@ namespace Modelec score_pub_ = node_->create_publisher("/strat/score", 10); - auto pos = nav_->GetHomePosition(); - home_point_ = Point(pos->x, pos->y, pos->theta); - if (nav_->CanGoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) - { - if (nav_->CanGoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) - { - status_ = MissionStatus::FAILED; - return; - } - } - nav_->RotateTo(home_point_); - go_timeout_ = node_->now(); status_ = MissionStatus::RUNNING; + step_ = ROTATE_TO_HOME; } void GoHomeMission::Update() @@ -47,6 +36,25 @@ namespace Modelec switch (step_) { case ROTATE_TO_HOME: + { + auto pos = nav_->GetHomePosition(); + home_point_ = Point(pos->x, pos->y, pos->theta); + if (nav_->CanGoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) + { + if (nav_->CanGoTo(home_point_.GetTakeBasePosition(), true) != Pathfinding::FREE) + { + status_ = MissionStatus::FAILED; + return; + } + } + nav_->RotateTo(home_point_); + + go_timeout_ = node_->now(); + + step_ = GO_HOME; + } + break; + case GO_HOME: { if (nav_->GoTo(home_point_.GetTakeBasePosition()) != Pathfinding::FREE) { @@ -58,34 +66,34 @@ namespace Modelec } go_timeout_ = node_->now(); + + step_ = GO_CLOSE; } - - step_ = GO_HOME; break; - case GO_HOME: - if ((node_->now() - start_time_).seconds() < 94) - { - break; - } - - go_timeout_ = node_->now(); - - nav_->GoTo(home_point_, true); - - step_ = GO_CLOSE; - break; - case GO_CLOSE: { - std_msgs::msg::Int64 msg; - msg.data = mission_score_; - score_pub_->publish(msg); + if ((node_->now() - start_time_).seconds() < 94) + { + break; + } + + nav_->GoTo(home_point_, true); + + go_timeout_ = node_->now(); step_ = DONE; - status_ = MissionStatus::DONE; - break; } + break; + case DONE: + { + std_msgs::msg::Int64 score_msg; + score_msg.data = mission_score_; + score_pub_->publish(score_msg); + + status_ = MissionStatus::DONE; + } + break; default: break; } diff --git a/src/modelec_strat/src/missions/take_mission.cpp b/src/modelec_strat/src/missions/take_mission.cpp new file mode 100644 index 0000000..6f8f688 --- /dev/null +++ b/src/modelec_strat/src/missions/take_mission.cpp @@ -0,0 +1,76 @@ +#include + +namespace Modelec { + TakeMission::TakeMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) + : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { + } + + void TakeMission::Start(rclcpp::Node::SharedPtr node) + { + node_ = node; + + go_timeout_ = node_->now(); + + status_ = MissionStatus::RUNNING; + step_ = GO_TO_TAKE; + } + + void TakeMission::Update() + { + if (!action_executor_->IsActionDone()) + { + return; + } + + if (!nav_->HasArrived()) + { + if ((node_->now() - go_timeout_).seconds() < 10) + { + return; + } + } + + switch (step_) + { + case GO_TO_TAKE: + { + + auto closestBox = nav_->GetClosestObstacle(nav_->GetCurrentPos()); + + auto pos = closestBox->GetOptimizedGetPos(nav_->GetCurrentPos()).GetTakeBasePosition(); + + nav_->GetPathfinding()->RemoveObstacle(closestBox->GetId()); + + nav_->GoToRotateFirst(pos, true, Pathfinding::FREE | Pathfinding::WALL); + + go_timeout_ = node_->now(); + } + + step_ = TAKE; + break; + case TAKE: + { + action_executor_->Up(); + deploy_time_ = node_->now(); + } + + step_ = DONE; + break; + case DONE: + status_ = MissionStatus::DONE; + break; + default: + break; + } + } + + void TakeMission::Clear() + { + } + + MissionStatus TakeMission::GetStatus() const + { + return status_; + } + +} \ No newline at end of file diff --git a/src/modelec_strat/src/missions/take_send_mission.cpp b/src/modelec_strat/src/missions/take_send_mission.cpp deleted file mode 100644 index a085e89..0000000 --- a/src/modelec_strat/src/missions/take_send_mission.cpp +++ /dev/null @@ -1,81 +0,0 @@ -#include - -namespace Modelec { - TakeSendMission::TakeSendMission(const std::shared_ptr& nav, const std::shared_ptr& action_executor) - : step_(GO_TO_TAKE), status_(MissionStatus::READY), nav_(nav), action_executor_(action_executor) { - } - - void TakeSendMission::Start(rclcpp::Node::SharedPtr node) - { - node_ = node; - - - nav_->GoToRotateFirst(2500, 1200, M_PI_2, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - - go_timeout_ = node_->now(); - - status_ = MissionStatus::RUNNING; - } - - void TakeSendMission::Update() - { - if (!action_executor_->IsActionDone()) - { - return; - } - - if (!nav_->HasArrived()) - { - if ((node_->now() - go_timeout_).seconds() < 10) - { - return; - } - } - - switch (step_) - { - case GO_TO_TAKE: - action_executor_->Take(); - deploy_time_ = node_->now(); - - step_ = TAKE; - break; - case TAKE: - if ((node_->now() - deploy_time_).seconds() >= 5) - { - step_ = WAIT_5S; - } - - break; - case WAIT_5S: - go_timeout_ = node_->now(); - nav_->GoToRotateFirst(2000, 700, 0, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE); - - step_ = GO_TO_SEND; - break; - case GO_TO_SEND: - action_executor_->Send(); - deploy_time_ = node_->now(); - - step_ = SEND; - break; - case SEND: - - step_ = DONE; - status_ = MissionStatus::DONE; - break; - default: - break; - } - } - - void TakeSendMission::Clear() - { - } - - MissionStatus TakeSendMission::GetStatus() const - { - return status_; - } - -} \ No newline at end of file diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 8685ebc..f61ae0d 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -125,12 +125,6 @@ namespace Modelec void NavigationHelper::Update() { - if ((node_->now() - last_odo_get_pos_time_).nanoseconds() > static_cast(1e8 * 2)) // 200ms - { - last_odo_get_pos_time_ = node_->now(); - std_msgs::msg::Empty empty_msg; - // odo_get_pos_pub_->publish(empty_msg); - } } void NavigationHelper::SendGoTo() @@ -511,7 +505,7 @@ namespace Modelec } std::shared_ptr NavigationHelper::GetClosestDepositeZone( - const PosMsg::SharedPtr& pos, int teamId, const std::vector& blacklistedId) + const PosMsg::SharedPtr& pos, const std::vector& blacklistedId, bool only_free) { std::shared_ptr closest_zone = nullptr; double score = std::numeric_limits::max(); @@ -520,10 +514,11 @@ namespace Modelec for (const auto& [id, zone] : deposite_zones_) { - if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find( - blacklistedId.begin(), blacklistedId.end(), id)) + if (blacklistedId.end() == std::find( + blacklistedId.begin(), blacklistedId.end(), id) + && (!only_free || !zone->Validate())) { - auto zonePoint = zone->GetNextPotPos().GetTakeBasePosition(); + auto zonePoint = zone->GetPosition(); double distance = Point::distance(posPoint, zonePoint); double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); double theta = std::abs(Point::angleDiff(posPoint, zonePoint)); @@ -746,33 +741,6 @@ namespace Modelec } } } - - /*if (await_rotate_) - { - await_rotate_ = false; - - waypoints_.clear(); - for (auto& w : send_back_waypoints_) - { - waypoints_.emplace_back(w); - } - // SendWaypoint(); - SendGoTo(); - } - else - { - if (!waypoint_queue_.empty()) - { - waypoint_pub_->publish(waypoint_queue_.front().ToMsg()); - waypoint_queue_.pop(); - - waypoints_[waypoints_.size() - waypoint_queue_.size() - 1].reached = true; - } - else - { - waypoints_.back().reached = true; - } - }*/ } void NavigationHelper::OnPos(const PosMsg::SharedPtr msg) diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index b803cf0..923ab72 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -261,7 +261,11 @@ namespace Modelec { const int goal_x = goal->x / cell_size_mm_x; const int goal_y = (grid_height_ - 1) - (goal->y / cell_size_mm_y); - RCLCPP_INFO(node_->get_logger(), "Start: (%d, %d), Goal: (%d, %d)", start_x, start_y, goal_x, goal_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, + (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); if (start_x < 0 || start_y < 0 || goal_x < 0 || goal_y < 0 || start_x >= grid_width_ || start_y >= grid_height_ || diff --git a/src/modelec_strat/src/strat_fms.cpp b/src/modelec_strat/src/strat_fms.cpp index 8bd815b..316587b 100644 --- a/src/modelec_strat/src/strat_fms.cpp +++ b/src/modelec_strat/src/strat_fms.cpp @@ -1,9 +1,11 @@ -#include -#include #include +#include +#include + #include -#include +#include +#include namespace Modelec { @@ -61,6 +63,11 @@ 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); + game_action_sequence_.push(State::TAKE_MISSION); + game_action_sequence_.push(State::FREE_MISSION); } void StratFMS::Init() @@ -119,8 +126,6 @@ namespace Modelec { auto now = this->now(); - nav_->Update(); - switch (state_) { case State::INIT: @@ -128,6 +133,10 @@ namespace Modelec { started_ = false; + std_msgs::msg::Bool start_odo_msg; + start_odo_msg.data = true; + start_odo_pub_->publish(start_odo_msg); + std_msgs::msg::Bool arm_msg; arm_msg.data = true; tir_arm_set_pub_->publish(arm_msg); @@ -140,10 +149,6 @@ namespace Modelec { rclcpp::sleep_for(std::chrono::milliseconds(300)); - std_msgs::msg::Bool start_odo_msg; - start_odo_msg.data = true; - start_odo_pub_->publish(start_odo_msg); - match_start_time_ = now; std_msgs::msg::Int64 msg; @@ -165,39 +170,57 @@ namespace Modelec Transition(State::STOP, "All missions done"); } - if (elapsed.seconds() < 70) + else if (elapsed.seconds() < 70) { - Transition(State::TAKE_SEND_MISSION, "Proceed to concert"); + Transition(State::SELECT_GAME_ACTION, "Selecting a game action"); } else { Transition(State::DO_GO_HOME, "Cleanup and finish match"); } - /*if (elapsed.seconds() >= 2) - { - Transition(State::DO_GO_HOME, "Go Home"); - }*/ - - /* - else - { - Transition(State::DO_GO_HOME, "Cleanup and finish match"); - }*/ break; } + case State::SELECT_GAME_ACTION: + { + if (game_action_sequence_.empty()) + { + Transition(State::DO_GO_HOME, "No more game actions"); + } + else + { + auto action = game_action_sequence_.front(); + game_action_sequence_.pop(); + Transition(action, "Selected game action"); + } + } - case State::TAKE_SEND_MISSION: + break; + case State::TAKE_MISSION: if (!current_mission_) { - current_mission_ = std::make_unique(nav_, action_executor_); + 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, "Take and Send done"); + Transition(State::SELECT_MISSION, "Take done"); + } + break; + + case State::FREE_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, "Free done"); } break;