modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

Develop ihm (#19)

Browse Source
This commit is contained in:
Ackimixs
2025-12-15 18:41:59 +01:00
committed by GitHub
parent 314ff524f5
commit 3494083870
45 changed files with 1687 additions and 999 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build.zsh
View File

@@ -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"

4
fastdds_setup.xml
View File

@@ -23,12 +23,12 @@
<!-- End repeat -->
<locator>
<udpv4>
<address>acki-windows.tail1f0d2.ts.net</address> <!-- ackimixs -->
<address>100.113.219.4</address> <!-- ackimixs -->
</udpv4>
</locator>
<locator>
<udpv4>
<address>modelec-robot.ayu-hops.ts.net</address>
<address>100.73.69.106</address>
</udpv4>
</locator>
</initialPeersList>

245
simulated_pcb/action.py
View File

@@ -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

5
simulated_pcb/odo.py
View File

@@ -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())

96
simulated_pcb/serial.modelec.py
View File

@@ -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)

150
simulated_pcb/serial.modelec.test.py
View File

@@ -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()

3
src/modelec_com/CMakeLists.txt
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>

55
src/modelec_com/include/modelec_com/pcb_action_interface.hpp
View File

@@ -6,13 +6,25 @@
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/bool.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state.hpp>
#include <modelec_interfaces/msg/action_servo_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state_array.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed.hpp>
#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<int, int> asc_value_mapper_;
std::map<int, std::map<int, double>> servo_pos_mapper_;
int asc_state_ = 0;
std::map<int, int> servo_value_;
std::map<int, double> servo_value_;
std::map<int, bool> relay_value_;
std::map<int, ServoTimedSet> servo_timed_buffer_;
rclcpp::TimerBase::SharedPtr servo_timed_timer_;
private:
void read(const std::string& msg) override;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_get_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_get_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_move_timed_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_move_timed_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_get_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_get_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_move_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_move_res_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_start_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_arm_pub_;
@@ -68,6 +82,7 @@ namespace Modelec
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {});
// TODO redo thos func to accept arrays, poc without them atm
void GetData(const std::string& elem, const std::vector<std::string>& data = {});
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
void SendMove(const std::string& elem, const std::vector<std::string>& data = {});

389
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -2,10 +2,11 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_utils/utils.hpp>
#include <fmt/core.h>
namespace Modelec
{
PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener()
PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface")
{
// Service to create a new serial listener
declare_parameter<std::string>("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<modelec_interfaces::msg::ActionAscPos>(
"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<modelec_interfaces::msg::ActionServoPos>(
servo_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPosArray>(
"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<modelec_interfaces::msg::ActionRelayState>(
relay_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionRelayStateArray>(
"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<modelec_interfaces::msg::ActionAscPos>(
"action/get/asc/res", 10);
servo_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
servo_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPosArray>(
"action/get/servo/res", 10);
relay_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
relay_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayStateArray>(
"action/get/relay/res", 10);
asc_set_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
@@ -54,21 +65,9 @@ namespace Modelec
SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)});
});
servo_set_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
"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<int>(msg->angle * 100))
});
});
asc_set_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
"action/set/asc/res", 10);
servo_set_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
"action/set/servo/res", 10);
asc_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"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<modelec_interfaces::msg::ActionServoPos>(
servo_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPosArray>(
"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<modelec_interfaces::msg::ActionRelayState>(
relay_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionRelayStateArray>(
"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<modelec_interfaces::msg::ActionAscPos>(
"action/move/asc/res", 10);
servo_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
servo_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPosArray>(
"action/move/servo/res", 10);
relay_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
relay_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayStateArray>(
"action/move/relay/res", 10);
tir_start_pub_ = this->create_publisher<std_msgs::msg::Empty>(
@@ -137,6 +150,91 @@ namespace Modelec
});
servo_move_timed_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoTimedArray>(
"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<modelec_interfaces::msg::ActionServoTimedArray>(
"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<int, double> 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<int>(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<std::string> 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<std::string>& data)
{
SendToPCB("GET", elem, data);

18
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -5,7 +5,7 @@
namespace Modelec
{
PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener()
PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface")
{
declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
declare_parameter<int>("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<modelec_interfaces::msg::OdometryPos>(
"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<std::string> 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);
}
}

5
src/modelec_core/launch/modelec.launch.py
View File

@@ -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",
}]

173
src/modelec_core/launch/test.modelec.launch.py
View File

@@ -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,
])

6
src/modelec_gui/CMakeLists.txt
View File

@@ -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
)

2
src/modelec_gui/include/modelec_gui/modelec_gui.hpp
View File

@@ -8,7 +8,7 @@
#include <modelec_gui/pages/test_map_page.hpp>
#include <modelec_gui/pages/odo_page.hpp>
#include <modelec_gui/pages/map_page.hpp>
#include <modelec_gui/pages/action_page.hpp>
#include <modelec_gui/pages/action_page.new.hpp>
#include <modelec_gui/pages/alim_page.hpp>

26
src/modelec_gui/include/modelec_gui/pages/action_page.hpp
View File

@@ -11,8 +11,8 @@
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_servo_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_relay_state_array.hpp>
#include <modelec_interfaces/msg/action_exec.hpp>
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<ActionWidget*> servo_actions_;
std::vector<bool> waiting_for_move_servo_;
QPushButton* test_button_;
bool test_ = false;
QHBoxLayout* relay_layout_;
@@ -76,14 +78,12 @@ namespace ModelecGUI
rclcpp::Publisher<ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Subscription<ActionAscPos>::SharedPtr asc_move_res_sub_;
rclcpp::Subscription<ActionServoPos>::SharedPtr servo_get_sub_;
rclcpp::Publisher<ActionServoPos>::SharedPtr servo_set_pub_;
rclcpp::Subscription<ActionServoPos>::SharedPtr servo_set_res_sub_;
rclcpp::Publisher<ActionServoPos>::SharedPtr servo_move_pub_;
rclcpp::Subscription<ActionServoPos>::SharedPtr servo_move_res_sub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_get_sub_;
rclcpp::Publisher<ActionServoPosArray>::SharedPtr servo_move_pub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_move_res_sub_;
rclcpp::Subscription<ActionRelayState>::SharedPtr relay_get_sub_;
rclcpp::Publisher<ActionRelayState>::SharedPtr relay_move_pub_;
rclcpp::Subscription<ActionRelayState>::SharedPtr relay_move_res_sub_;
rclcpp::Subscription<ActionRelayStateArray>::SharedPtr relay_get_sub_;
rclcpp::Publisher<ActionRelayStateArray>::SharedPtr relay_move_pub_;
rclcpp::Subscription<ActionRelayStateArray>::SharedPtr relay_move_res_sub_;
};
} // namespace Modelec

102
src/modelec_gui/include/modelec_gui/pages/action_page.new.hpp Normal file
View File

@@ -0,0 +1,102 @@
#pragma once
#include <QLabel>
#include <QWidget>
#include <QPushButton>
#include <QLineEdit>
#include <QVBoxLayout>
#include <QSpinBox>
#include <QLabel>
#include <modelec_gui/widget/action_widget.hpp>
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_relay_state_array.hpp>
#include <modelec_interfaces/msg/action_exec.hpp>
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<ActionExec>::SharedPtr action_exec_pub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_get_sub_;
rclcpp::Publisher<ActionServoPosArray>::SharedPtr servo_move_pub_;
rclcpp::Subscription<ActionServoPosArray>::SharedPtr servo_move_res_sub_;
rclcpp::Publisher<ActionServoTimedArray>::SharedPtr servo_timed_move_pub_;
rclcpp::Subscription<ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
};
} // namespace Modelec

4
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -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);

116
src/modelec_gui/src/pages/action_page.cpp
View File

@@ -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<bool>(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<ActionServoPos>(
"action/get/servo", 10, [this](const ActionServoPos::SharedPtr msg)
servo_get_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr msg)
{
if (static_cast<int>(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<int>(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<ActionServoPos>(
"action/set/servo", 10);
servo_set_res_sub_ = node_->create_subscription<ActionServoPos>(
"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<ActionServoPos>(
servo_move_pub_ = node_->create_publisher<ActionServoPosArray>(
"action/move/servo", 10);
servo_move_res_sub_ = node_->create_subscription<ActionServoPos>(
"action/move/servo/res", 10, [this](const ActionServoPos::SharedPtr msg)
servo_move_res_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr msg)
{
if (static_cast<int>(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<int>(servo_actions_.size()) > item.id && servo_actions_[item.id] != nullptr)
{
servo_actions_[item.id]->setDisabled(false);
}
}
});
relay_get_sub_ = node_->create_subscription<ActionRelayState>(
"action/get/relay", 10, [this](const ActionRelayState::SharedPtr msg)
relay_get_sub_ = node_->create_subscription<ActionRelayStateArray>(
"action/get/relay", 10, [this](const ActionRelayStateArray::SharedPtr msg)
{
if (static_cast<int>(relay_values_.size()) > msg->id)
for (const auto& item : msg->items)
{
relay_values_[msg->id] = msg->state;
if (static_cast<int>(relay_buttons_.size()) > item.id && relay_buttons_[item.id] != nullptr)
{
relay_values_[item.id] = item.state;
}
}
});
relay_move_pub_ = node_->create_publisher<ActionRelayState>(
relay_move_pub_ = node_->create_publisher<ActionRelayStateArray>(
"action/move/relay", 10);
relay_move_res_sub_ = node_->create_subscription<ActionRelayState>(
"action/move/relay/res", 10, [this](const ActionRelayState::SharedPtr msg)
relay_move_res_sub_ = node_->create_subscription<ActionRelayStateArray>(
"action/move/relay/res", 10, [this](const ActionRelayStateArray::SharedPtr msg)
{
if (static_cast<int>(relay_buttons_.size()) > msg->id && relay_buttons_[msg->id] != nullptr && static_cast<int>(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<int>(relay_buttons_.size()) > item.id && relay_buttons_[item.id] != nullptr)
{
relay_buttons_[item.id]->setDisabled(false);
relay_values_[item.id] = item.state;
}
}
});
}

295
src/modelec_gui/src/pages/action_page.new.cpp Normal file
View File

@@ -0,0 +1,295 @@
#include <modelec_gui/pages/action_page.new.hpp>
namespace ModelecGUI
{
ActionPage::ActionPage(rclcpp::Node::SharedPtr node, QWidget* parent) :
QWidget(parent),
node_(node)
{
action_exec_pub_ = node_->create_publisher<ActionExec>(
"action/exec", 10);
servo_get_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/get/servo", 10, [this](const ActionServoPosArray::SharedPtr)
{
});
servo_move_pub_ = node_->create_publisher<ActionServoPosArray>(
"action/move/servo", 10);
servo_move_res_sub_ = node_->create_subscription<ActionServoPosArray>(
"action/move/servo/res", 10, [this](const ActionServoPosArray::SharedPtr)
{
});
servo_timed_move_pub_ = node_->create_publisher<ActionServoTimedArray>(
"action/move/servo/timed", 10);
servo_timed_move_res_sub_ = node_->create_subscription<ActionServoTimedArray>(
"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;
}

12
src/modelec_interfaces/CMakeLists.txt
View File

@@ -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"

74
src/modelec_interfaces/msg/Action/ActionExec.msg
View File

@@ -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

1
src/modelec_interfaces/msg/Action/ActionServoPos.msg
View File

@@ -1,4 +1,3 @@
int32 id
int32 pos
float64 angle
bool success

5
src/modelec_interfaces/msg/Action/ActionServoTimed.msg Normal file
View File

@@ -0,0 +1,5 @@
int32 id
float64 start_angle
float64 end_angle
float64 duration_s
bool success

2
src/modelec_interfaces/msg/Action/Array/ActionRelayStateArray.msg Normal file
View File

@@ -0,0 +1,2 @@
ActionRelayState[] items
bool success

2
src/modelec_interfaces/msg/Action/Array/ActionServoPosArray.msg Normal file
View File

@@ -0,0 +1,2 @@
ActionServoPos[] items
bool success

2
src/modelec_interfaces/msg/Action/Array/ActionServoTimedArray.msg Normal file
View File

@@ -0,0 +1,2 @@
ActionServoTimed[] items
bool success

2
src/modelec_strat/CMakeLists.txt
View File

@@ -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

20
src/modelec_strat/data/deposite_zone.xml
View File

@@ -1,32 +1,32 @@
<Map>
<DepositeZone id="0">
<Pos x="1150" y="1350" theta="0" w="200" h="200" />
<Pos x="1250" y="1450" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="1">
<Pos x="1650" y="1350" theta="0" w="200" h="200" />
<Pos x="1750" y="1450" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="2">
<Pos x="0" y="700" theta="0" w="200" h="200" />
<Pos x="100" y="800" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="3">
<Pos x="700" y="700" theta="0" w="200" h="200" />
<Pos x="800" y="800" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="4">
<Pos x="1400" y="700" theta="0" w="200" h="200" />
<Pos x="1500" y="800" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="5">
<Pos x="2100" y="700" theta="0" w="200" h="200" />
<Pos x="2200" y="800" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="6">
<Pos x="2800" y="700" theta="0" w="200" h="200" />
<Pos x="2900" y="800" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="7">
<Pos x="600" y="0" theta="0" w="200" h="200" />
<Pos x="700" y="100" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="8">
<Pos x="1400" y="0" theta="0" w="200" h="200" />
<Pos x="1500" y="100" theta="0" w="200" h="200" />
</DepositeZone>
<DepositeZone id="9">
<Pos x="2200" y="0" theta="0" w="200" h="200" />
<Pos x="2300" y="100" theta="0" w="200" h="200" />
</DepositeZone>
</Map>

18
src/modelec_strat/data/obstacles.xml
View File

@@ -1,27 +1,27 @@
<Obstacles>
<!-- TOP -->
<!--<Obstacle id="1" x="1500" y="1800" theta="1.5708" width="1800" height="400" type="estrade"/>-->
<!--<Obstacle id="2" x="1500" y="1775" theta="1.5708" width="900" height="450" type="estrade"/>
<Obstacle id="3" x="2150" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>-->
<Obstacle id="1" x="1500" y="1800" theta="1.5708" width="1800" height="400" type="estrade"/>
<Obstacle id="2" x="1500" y="1775" theta="1.5708" width="900" height="450" type="estrade"/>
<Obstacle id="3" x="2150" y="1900" theta="1.5708" width="400" height="200" type="estrade"/>
<!-- Box TOP TO BOTTOM LEFT -->
<!--<Box id="10" x="175" y="1200" theta="0" width="200" height="150" type="box" />
<Box id="10" x="175" y="1200" theta="0" width="200" height="150" type="box" />
<Box id="11" x="1150" y="800" theta="1.5708" width="200" height="150" type="box" >
<possible-angle theta="-1.5708" />
</Box>
<Box id="12" x="175" y="400" theta="0" width="200" height="150" type="box" />
<Box id="13" x="1100" y="175" theta="1.5708" width="200" height="150" type="box" />-->
<Box id="13" x="1100" y="175" theta="1.5708" width="200" height="150" type="box" />
<!-- Box TOP TO BOTTOM RIGHT -->
<!--<Box id="20" x="2825" y="1200" theta="3.1415" width="200" height="150" type="box" />
<Box id="20" x="2825" y="1200" theta="3.1415" width="200" height="150" type="box" />
<Box id="21" x="1850" y="800" theta="1.5708" width="200" height="150" type="box">
<possible-angle theta="-1.5708" />
</Box>
<Box id="22" x="2825" y="400" theta="3.1415" width="200" height="150" type="box"/>
<Box id="23" x="1900" y="175" theta="-1.5708" width="200" height="150" type="box"/>-->
<Box id="23" x="1900" y="175" theta="-1.5708" width="200" height="150" type="box"/>
<!-- PAMI Start -->
<!--<Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>
<Obstacle id="31" x="2925" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>-->
<Obstacle id="30" x="75" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>
<Obstacle id="31" x="2925" y="1750" theta="1.5708" width="150" height="450" type="pami-start"/>
</Obstacles>

94
src/modelec_strat/include/modelec_strat/action_executor.hpp
View File

@@ -2,8 +2,9 @@
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state.hpp>
#include <modelec_interfaces/msg/action_servo_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state_array.hpp>
#include <modelec_interfaces/msg/action_servo_pos_array.hpp>
#include <modelec_interfaces/msg/action_servo_timed_array.hpp>
#include <modelec_interfaces/msg/action_exec.hpp>
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<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_move_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_timed_move_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPosArray>::SharedPtr servo_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayStateArray>::SharedPtr relay_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoTimedArray>::SharedPtr servo_timed_move_res_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionExec>::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_;
};

36
src/modelec_strat/include/modelec_strat/deposite_zone.hpp
View File

@@ -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<int>::min(), std::numeric_limits<int>::min(), 0);
return pot_queue_.front();
}
void Validate(bool valid);
Point ValidNextPotPos()
{
if (pot_queue_.empty())
return Point(std::numeric_limits<int>::min(), std::numeric_limits<int>::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<Point> pot_queue_;
bool has_box_ = false;
};
}

37
src/modelec_strat/include/modelec_strat/missions/free_mission.hpp Normal file
View File

@@ -0,0 +1,37 @@
#pragma once
#include <modelec_strat/missions/mission_base.hpp>
#include <modelec_strat/navigation_helper.hpp>
#include <modelec_strat/action_executor.hpp>
namespace Modelec {
class FreeMission : public Mission {
public:
FreeMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& 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<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Node::SharedPtr node_;
std::shared_ptr<DepositeZone> target_deposite_zone_;
rclcpp::Time go_timeout_;
rclcpp::Time deploy_time_;
};
}

10
src/modelec_strat/include/modelec_strat/missions/take_send_mission.hpp → src/modelec_strat/include/modelec_strat/missions/take_mission.hpp
View File

@@ -5,32 +5,28 @@
#include <modelec_strat/action_executor.hpp>
namespace Modelec {
class TakeSendMission : public Mission {
class TakeMission : public Mission {
public:
TakeSendMission(const std::shared_ptr<NavigationHelper>& nav,
TakeMission(const std::shared_ptr<NavigationHelper>& nav,
const std::shared_ptr<ActionExecutor>& 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<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;
rclcpp::Time go_home_time_;
rclcpp::Node::SharedPtr node_;
rclcpp::Time go_timeout_;

31
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -87,8 +87,12 @@ namespace Modelec
bool LoadDepositeZoneFromXML(const std::string& filename);
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId,
const std::vector<int>& blacklistedId = {});
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr& pos,
const std::vector<int>& blacklistedId = {}, bool only_free = false);
template <typename T,
typename = std::enable_if_t<std::is_base_of<Obstacle, T>::value>>
std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos) const;
PosMsg::SharedPtr GetHomePosition();
@@ -171,4 +175,27 @@ namespace Modelec
rclcpp::Time last_odo_get_pos_time_;
};
template <typename T, typename>
std::shared_ptr<T> NavigationHelper::GetClosestObstacle(const PosMsg::SharedPtr& pos) const
{
std::shared_ptr<T> closest_obstacle = nullptr;
auto robotPos = Point(pos->x, pos->y, pos->theta);
float distance = std::numeric_limits<float>::max();
for (const auto& obstacle : GetPathfinding()->GetObstacles())
{
if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second))
{
auto dist = Point::distance(robotPos, obs->GetPosition());
if (dist < distance)
{
distance = dist;
closest_obstacle = obs;
}
}
}
return closest_obstacle;
}
}

4
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -79,6 +79,10 @@ namespace Modelec
[[nodiscard]] std::shared_ptr<Obstacle> GetObstacle(int id) const;
std::map<int, std::shared_ptr<Obstacle>> GetObstacles() const {
return obstacle_map_;
}
void RemoveObstacle(int id);
void AddObstacle(const std::shared_ptr<Obstacle>& obstacle);

6
src/modelec_strat/include/modelec_strat/strat_fms.hpp
View File

@@ -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<Mission> current_mission_;
int team_id_ = 0;
std::queue<State> game_action_sequence_;
std::shared_ptr<NavigationHelper> nav_;
std::shared_ptr<ActionExecutor> action_executor_;

159
src/modelec_strat/src/action_executor.cpp
View File

@@ -9,8 +9,8 @@ namespace Modelec
ActionExecutor::ActionExecutor(const rclcpp::Node::SharedPtr& node) : node_(node)
{
asc_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionAscPos>("/action/move/asc", 10);
servo_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPos>("/action/move/servo", 10);
relay_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionRelayState>("/action/move/relay", 10);
servo_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionServoPosArray>("/action/move/servo", 10);
relay_move_pub_ = node_->create_publisher<modelec_interfaces::msg::ActionRelayStateArray>("/action/move/relay", 10);
asc_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"/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<modelec_interfaces::msg::ActionServoPos>(
"/action/move/servo/res", 10, [this](const modelec_interfaces::msg::ActionServoPos::SharedPtr)
servo_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoPosArray>(
"/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<modelec_interfaces::msg::ActionRelayState>(
"/action/move/relay/res", 10, [this](const modelec_interfaces::msg::ActionRelayState::SharedPtr)
relay_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionRelayStateArray>(
"/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<modelec_interfaces::msg::ActionServoTimedArray>("/action/move/servo/timed", 10);
servo_timed_move_res_sub_ = node_->create_subscription<modelec_interfaces::msg::ActionServoTimedArray>(
"/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<Action>(n * 10 + (front ? 0 : 100));
action_done_ = false;
step_running_ = 0;
step_.push(static_cast<Step>(n * 10 + (front ? 0 : 100)));
Update();
}
}
void ActionExecutor::Free(bool front, int n) {
if (action_done_)
{
action_ = static_cast<Action>(1 + n * 10 + (front ? 0 : 100));
action_done_ = false;
step_running_ = 0;
step_.push(static_cast<Step>(1 + n * 10 + (front ? 0 : 100)));
Update();
}

41
src/modelec_strat/src/deposite_zone.cpp
View File

@@ -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_; }
}

86
src/modelec_strat/src/missions/free_mission.cpp Normal file
View File

@@ -0,0 +1,86 @@
#include <modelec_strat/missions/free_mission.hpp>
namespace Modelec {
FreeMission::FreeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& 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_;
}
}

72
src/modelec_strat/src/missions/go_home_mission.cpp
View File

@@ -17,21 +17,10 @@ namespace Modelec
score_pub_ = node_->create_publisher<std_msgs::msg::Int64>("/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;
}

76
src/modelec_strat/src/missions/take_mission.cpp Normal file
View File

@@ -0,0 +1,76 @@
#include <modelec_strat/missions/take_mission.hpp>
namespace Modelec {
TakeMission::TakeMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& 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<BoxObstacle>(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_;
}
}

81
src/modelec_strat/src/missions/take_send_mission.cpp
View File

@@ -1,81 +0,0 @@
#include <modelec_strat/missions/take_send_mission.hpp>
namespace Modelec {
TakeSendMission::TakeSendMission(const std::shared_ptr<NavigationHelper>& nav, const std::shared_ptr<ActionExecutor>& 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_;
}
}

42
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -125,12 +125,6 @@ namespace Modelec
void NavigationHelper::Update()
{
if ((node_->now() - last_odo_get_pos_time_).nanoseconds() > static_cast<int64_t>(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<DepositeZone> NavigationHelper::GetClosestDepositeZone(
const PosMsg::SharedPtr& pos, int teamId, const std::vector<int>& blacklistedId)
const PosMsg::SharedPtr& pos, const std::vector<int>& blacklistedId, bool only_free)
{
std::shared_ptr<DepositeZone> closest_zone = nullptr;
double score = std::numeric_limits<double>::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)

6
src/modelec_strat/src/pathfinding.cpp
View File

@@ -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_ ||

71
src/modelec_strat/src/strat_fms.cpp
View File

@@ -1,9 +1,11 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_strat/strat_fms.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_strat/missions/go_home_mission.hpp>
#include <modelec_strat/missions/take_send_mission.hpp>
#include <modelec_strat/missions/take_mission.hpp>
#include <modelec_strat/missions/free_mission.hpp>
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<TakeSendMission>(nav_, action_executor_);
current_mission_ = std::make_unique<TakeMission>(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<FreeMission>(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;