use std::{thread, sync::{mpsc, Arc}, time::Duration};
use zmq;
use serde_json;
use tokio::sync::Mutex;
pub mod errors {
#[derive(Debug, Clone)]
pub enum RskError {
OtherError(String),
GameControllerRefusal(String)
}
impl<T: RskErrorable> From<T> for RskError {
fn from(t: T) -> Self {
t.to_rsk_err()
}
}
pub trait RskErrorable {
fn to_rsk_err(&self) -> RskError;
}
impl<T: ToString> RskErrorable for T {
fn to_rsk_err(&self) -> RskError {
RskError::OtherError(self.to_string())
}
}
pub type RskResult<T> = Result<T, RskError>;
}
use errors::*;
fn angle_wrap(angle: f32) -> f32 {
let mut angle = angle;
while angle > std::f32::consts::PI {
angle -= 2.0 * std::f32::consts::PI;
}
while angle < -std::f32::consts::PI {
angle += 2.0 * std::f32::consts::PI;
}
angle
}
#[derive(Debug, Clone, Copy)]
pub enum Team {
Green,
Blue
}
impl ToString for Team {
fn to_string(&self) -> String {
match self {
Self::Green => "green".to_string(),
Self::Blue => "blue".to_string()
}
}
}
#[derive(Debug, Clone, Copy)]
pub enum RobotNumber {
One,
Two
}
impl ToString for RobotNumber {
fn to_string(&self) -> String {
match self {
Self::One => "1".to_string(),
Self::Two => "2".to_string()
}
}
}
#[derive(Debug, Clone, Copy)]
pub enum Command {
Kick(f32),
Control(AbsPose),
Teleport(AbsPose),
Leds(GameDataLedsValue),
Beep(u16, u16)
}
impl ToString for Command {
fn to_string(&self) -> String {
match self {
Self::Kick(a) => format!(r#"["kick", {a}]"#),
Self::Control(p) => format!(r#"["control", {}, {}, {}]"#, p.0, p.1, p.get_theta()),
Self::Teleport(p) => format!(r#"["teleport", {}, {}, {}]"#, p.0, p.1, p.get_theta()),
Self::Leds(a) => format!(r#"["leds", {}, {}, {}]"#, a.0, a.1, a.2),
Self::Beep(a, b) => format!(r#"["beep", {a}, {b}]"#)
}
}
}
#[derive(Debug, Clone, Copy)]
pub struct Destination {
pub pose: AbsPose,
pub speed: f32,
pub rotation_speed: f32
}
impl Destination {
pub fn new(pose: AbsPose, speed: f32, rotation_speed: f32) -> Self {
Self {
pose,
speed,
rotation_speed
}
}
pub fn from_pose(pose: AbsPose) -> Self {
Self::new(pose, 3.0, 10.0)
}
pub fn to_control_command(&self, actual_pose:AbsPose) -> Command {
let mut vector = MoveVector::new(self.pose.0, self.pose.1);
vector.0 -= actual_pose.0;
vector.1 -= actual_pose.1;
let cos = angle_wrap(-actual_pose.2).cos();
let sin = angle_wrap(-actual_pose.2).sin();
vector = MoveVector::new(
vector.0 * cos - vector.1 * sin,
vector.0 * sin + vector.1 * cos
);
let distance = (actual_pose - self.pose).norm();
let mut speed = self.speed;
let t = distance;
speed = (2.0 * t * (1.0 - t) + t.powi(2)) * speed;
vector = MoveVector::new(
(vector.0 * speed) / vector.norm(),
(vector.1 * speed) / vector.norm()
);
let speed = self.rotation_speed;
let mut angle = angle_wrap(self.pose.2 - actual_pose.2);
angle *= speed;
Command::Control(AbsPose(
vector.0 + actual_pose.0,
vector.1 + actual_pose.1,
angle
))
}
}
pub struct Robot<'a> {
pub position: Position,
pub orientation: Orientation,
pub leds: GameDataLedsValue,
pub penalized: bool,
pub penalized_remaining: Option<u32>,
pub penalized_reason: Option<String>,
pub preempted: bool,
pub preemption_reasons: Vec<String>,
pub number: RobotNumber,
pub team: Team,
pub client: &'a Client
}
impl std::fmt::Debug for Robot<'_> {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
f.debug_struct("Robot")
.field("position", &self.position)
.field("orientation", &self.orientation)
.field("leds", &self.leds)
.field("penalized", &self.penalized)
.field("penalized_remaining", &self.penalized_remaining)
.field("penalized_reason", &self.penalized_reason)
.field("preempted", &self.preempted)
.field("preemption_reasons", &self.preemption_reasons)
.field("number", &self.number)
.field("team", &self.team)
.finish()
}
}
impl Robot<'_> {
pub async fn send_command(&self, command: Command) -> RskResult<()> {
self.client.send_command(self.team, self.number, command).await
}
pub fn refresh(&mut self) -> RskResult<()> {
let robot = self.client.get_robot(self.team, self.number)?;
self.position = robot.position;
self.orientation = robot.orientation;
self.leds = robot.leds;
self.penalized = robot.penalized;
self.penalized_remaining = robot.penalized_remaining;
self.penalized_reason = robot.penalized_reason;
self.preempted = robot.preempted;
self.preemption_reasons = robot.preemption_reasons;
Ok(())
}
pub fn get_pose(&mut self) -> RskResult<AbsPose> {
self.refresh()?;
Ok(self.get_cache_pose())
}
pub fn get_cache_pose(&self) -> AbsPose {
AbsPose::new(
self.position.0,
self.position.1,
self.orientation.0
)
}
pub async fn get_destination(&self) -> RskResult<Option<Destination>> {
self.client.get_destination(self.team, self.number).await
}
pub async fn is_arrived(&self) -> RskResult<bool> {
self.client.is_arrived(self.team, self.number).await
}
pub async fn goto(&self, destination: Destination) -> RskResult<()> {
self.client.goto(self.team, self.number, destination).await
}
pub async fn stop(&self) -> RskResult<()> {
self.client.stop(self.team, self.number).await
}
pub async fn goto_blocking(&self, destination: Destination) -> RskResult<()> {
self.client.goto_blocking(self.team, self.number, destination).await
}
pub async fn kick(&self, power: f32) -> RskResult<()> {
self.client.send_command(self.team, self.number, Command::Kick(power)).await
}
}
pub mod game_data_structs {
use serde::{Serialize, Deserialize};
use super::{Team, Client, errors::RskResult, angle_wrap};
pub trait Pose: Serialize + for<'a> Deserialize<'a> + std::fmt::Debug + Clone + Copy + PartialEq + std::ops::Sub {
fn new(x: f32, y: f32, theta: f32) -> Self;
fn get_x(&self) -> f32;
fn get_y(&self) -> f32;
fn get_theta(&self) -> f32;
fn set_x(&mut self, x: f32);
fn set_y(&mut self, y: f32);
fn set_theta(&mut self, theta: f32);
}
#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq)]
pub struct AbsPose(
pub f32,
pub f32,
pub f32
);
impl Pose for AbsPose {
fn new(x: f32, y: f32, theta: f32) -> Self {AbsPose(x, y, angle_wrap(theta))}
fn get_x(&self) -> f32 {self.0}
fn get_y(&self) -> f32 {self.1}
fn get_theta(&self) -> f32 {self.2}
fn set_x(&mut self, x: f32) {self.0 = x;}
fn set_y(&mut self, y: f32) {self.1 = y;}
fn set_theta(&mut self, theta: f32) {
self.2 = angle_wrap(theta);
}
}
impl std::ops::Sub for AbsPose {
type Output = MoveVector;
fn sub(self, other: AbsPose) -> MoveVector {
MoveVector (
other.get_x() - self.get_x(),
other.get_y() - self.get_y()
)
}
}
impl std::ops::Add<MoveVector> for AbsPose {
type Output = AbsPose;
fn add(self, other: MoveVector) -> AbsPose {
AbsPose (
self.get_x() + other.0,
self.get_y() + other.1,
self.get_theta()
)
}
}
impl AbsPose {
pub fn get_rel(&self, team:Team, client:&Client) -> RskResult<RelPose> {
let gd = client.get_game_data()?;
let x_positive = match team {
Team::Blue => gd.referee.teams.blue.x_positive,
Team::Green => gd.referee.teams.green.x_positive
};
let mut rel_pose = RelPose::new(
self.get_x(),
self.get_y(),
self.get_theta()
);
if x_positive {
rel_pose.set_x(rel_pose.get_x() * -1.0);
rel_pose.set_theta(rel_pose.get_theta() + std::f32::consts::PI);
}
Ok(rel_pose)
}
}
#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq)]
pub struct RelPose (
pub f32,
pub f32,
pub f32
);
impl Pose for RelPose {
fn new(x: f32, y: f32, theta: f32) -> Self {
RelPose(x, y, angle_wrap(theta))
}
fn get_x(&self) -> f32 {self.0}
fn get_y(&self) -> f32 {self.1}
fn get_theta(&self) -> f32 {self.2}
fn set_x(&mut self, x: f32) {self.0 = x;}
fn set_y(&mut self, y: f32) {self.1 = y;}
fn set_theta(&mut self, theta: f32) {
self.2 = angle_wrap(theta);
}
}
impl std::ops::Sub for RelPose {
type Output = MoveVector;
fn sub(self, other: RelPose) -> MoveVector {
MoveVector (
other.get_x() - self.get_x(),
other.get_y() - self.get_y()
)
}
}
impl RelPose {
pub fn get_abs(&self, team:Team, client:&Client) -> RskResult<AbsPose> {
let gd = client.get_game_data()?;
let x_positive = match team {
Team::Blue => gd.referee.teams.blue.x_positive,
Team::Green => gd.referee.teams.green.x_positive
};
let mut abs_pose = AbsPose::new(
self.get_x(),
self.get_y(),
self.get_theta()
);
if x_positive {
abs_pose.set_x(abs_pose.get_x() * -1.0);
abs_pose.set_theta(abs_pose.get_theta() + std::f32::consts::PI);
}
Ok(abs_pose)
}
}
#[derive(Serialize, Deserialize, Debug, Clone, Copy)]
pub struct Position(
pub f32,
pub f32
);
impl std::ops::Sub for Position {
type Output = MoveVector;
fn sub(self, other: Position) -> MoveVector {
MoveVector (
other.0 - self.0,
other.1 - self.1
)
}
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct Orientation(
pub f32
);
impl Orientation {
pub fn new(theta: f32) -> Self {
Orientation(angle_wrap(theta))
}
}
#[derive(Serialize, Deserialize, Debug, Clone, Copy, PartialEq)]
pub struct MoveVector (
pub f32,
pub f32
);
impl std::ops::Mul<f32> for MoveVector {
type Output = MoveVector;
fn mul(self, other: f32) -> MoveVector {
MoveVector (
self.0 * other,
self.1 * other
)
}
}
impl std::ops::Div<f32> for MoveVector {
type Output = MoveVector;
fn div(self, other: f32) -> MoveVector {
MoveVector (
self.0 / other,
self.1 / other
)
}
}
impl MoveVector {
pub fn new(x: f32, y: f32) -> Self {
MoveVector(x, y)
}
pub fn norm(&self) -> f32 {
(self.0.powi(2) + self.1.powi(2)).sqrt()
}
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataMarkersRobot {
pub position: Position,
pub orientation: Orientation
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataMarkers {
pub green1: GameDataMarkersRobot,
pub green2: GameDataMarkersRobot,
pub blue1: GameDataMarkersRobot,
pub blue2: GameDataMarkersRobot
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataRefereeTeamRobot {
pub penalized: bool,
pub penalized_remaining: Option<u32>,
pub penalized_reason: Option<String>,
pub preempted: bool,
pub preemption_reasons: Vec<String>
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataRefereeTeamRobots {
pub one: GameDataRefereeTeamRobot,
pub two: GameDataRefereeTeamRobot
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataRefereeTeam {
pub name: String,
pub score: u8,
pub x_positive: bool,
pub robots: GameDataRefereeTeamRobots
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataRefereeTeams {
pub green: GameDataRefereeTeam,
pub blue: GameDataRefereeTeam
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataReferee {
pub game_is_running: bool,
pub game_paused: bool,
pub halftime_is_running: bool,
pub timer: i32,
pub game_state_msg: String,
pub teams: GameDataRefereeTeams,
pub referee_history_sliced: Vec<Vec<serde_json::Value>>
}
#[derive(Serialize, Deserialize, Debug, Clone, Copy)]
pub struct GameDataLedsValue(
pub u8,
pub u8,
pub u8
);
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameDataLeds {
pub green1: GameDataLedsValue,
pub green2: GameDataLedsValue,
pub blue1: GameDataLedsValue,
pub blue2: GameDataLedsValue
}
#[derive(Serialize, Deserialize, Debug, Clone)]
pub struct GameData {
pub markers: GameDataMarkers,
pub ball: Position,
pub referee: GameDataReferee,
pub leds: GameDataLeds,
pub simulated: bool
}
}
use game_data_structs::*;
#[derive(Debug, Clone, Copy)]
pub struct Destinations {
pub blue1: Option<Destination>,
pub blue2: Option<Destination>,
pub green1: Option<Destination>,
pub green2: Option<Destination>
}
#[derive(Clone)]
pub struct Client {
pub key: String,
pub host: String,
pub team: Team,
cmd_sender: mpsc::SyncSender<(Team, RobotNumber, Command)>,
game_data: Arc<std::sync::Mutex<Option<GameData>>>,
pub destinations: Arc<Mutex<Destinations>>
}
impl Client {
pub fn new(key:String, host:String, team:Team) -> RskResult<Client> {
let (
cmd_sender,
rx
) = mpsc::sync_channel::<(Team, RobotNumber, Command)>(10);
let key_clone = key.clone();
let host_clone = host.clone();
thread::spawn(move || {
loop {
match Client::req_process(&key_clone, &host_clone, &rx) {
Ok(_) => (),
Err(e) => println!("Client::req_process() :\n{}", match e {
RskError::OtherError(s) => s,
RskError::GameControllerRefusal(s) => s
})
}
}
});
let game_data = Arc::new(std::sync::Mutex::new(None));
let game_data_clone = game_data.clone();
let host_clone = host.clone();
thread::spawn(move || {
loop {
match Client::sub_process(&host_clone, game_data_clone.clone()) {
Ok(_) => (),
Err(e) => println!("Client::sub_process() :\n{}", match e {
RskError::OtherError(s) => s,
RskError::GameControllerRefusal(s) => s
})
}
}
});
let client = Client {
key,
host,
team,
cmd_sender,
game_data,
destinations: Arc::new(Mutex::new(Destinations {
blue1: None,
blue2: None,
green1: None,
green2: None
}))
};
let client_clone = client.clone();
tokio::spawn(async move {
loop {
if let Err(e) = Self::goto_process(client_clone.clone(), team, RobotNumber::One).await {
println!("Error in goto process 1: {:?}", e);
}
}
});
let client_clone = client.clone();
tokio::spawn(async move {
loop {
if let Err(e) = Self::goto_process(client_clone.clone(), team, RobotNumber::Two).await {
println!("Error in goto process 2: {:?}", e);
}
}
});
let mut gd = None;
while let None = gd {
gd = (*client.game_data.lock()?).clone();
}
Ok(client)
}
pub async fn send_command(&self, team:Team, number:RobotNumber, command:Command) -> RskResult<()> {
self.cmd_sender.send((team, number, command))?;
Ok(())
}
pub fn req_process(key: &str, host: &str, rx: &mpsc::Receiver<(Team, RobotNumber, Command)>) -> RskResult<()> {
let ctx = zmq::Context::new();
let req = ctx.socket(zmq::REQ)?;
req.connect(&("tcp://".to_string() + host + ":7558"))?;
loop {
let (
team,
number,
command
) = rx.recv()?;
req.send(
zmq::Message::from(
&format!(r#"["{0}", "{1}", {2}, {3}]"#, key, team.to_string(), number.to_string(), command.to_string())
),
0
)?;
let response = match req.recv_string(0)? {
Err(e) => return Err(format!("Client.send_command() : the received message is not a string. raw data : {:?}", e).to_rsk_err()),
Ok(v) => v
};
let response: Vec<serde_json::Value> = serde_json::from_str(&response)?;
match response.get(0).unwrap_or(&serde_json::Value::Null) {
serde_json::Value::Bool(b) => {
if !b {
let message = match response.get(1).unwrap_or(&serde_json::Value::Null) {
serde_json::Value::String(s) => s,
_ => return Err("Client.send_command() : the second index of the received array is not a string".to_rsk_err())
};
return Err(("Client.send_command() : the server returned an error : ".to_string() + message).to_rsk_err());
}
},
serde_json::Value::Number(_) => {
println!("Client.reqprocess() : {}", response.get(1)
.unwrap_or(
&serde_json::Value::String("Client.send_command() : the second index of the received array is not a string".to_string())
)
);
},
_ => return Err("Client.send_command() : the first index of the received array is not a boolean".to_rsk_err())
};
}
}
pub fn sub_process(host: &str, game_data: Arc<std::sync::Mutex<Option<GameData>>>) -> RskResult<()> {
let ctx = zmq::Context::new();
let sub = ctx.socket(zmq::SUB)?;
sub.connect(&("tcp://".to_string() + host + ":7557"))?;
sub.set_subscribe(b"")?;
loop {
let mut response = match sub.recv_string(0)? {
Err(e) => return Err(format!("the received message is not a string.\n\nraw data : {:?}", e).to_rsk_err()),
Ok(v) => v
};
response = response.replace(r#""1":"#, r#""one":"#);
response = response.replace(r#""2":"#, r#""two":"#);
let received_game_data: GameData = match serde_json::from_str(&response) {
Err(e) => return Err(format!("the received message is not a valid json.\n\ninitial error : {:?}\n\nraw game data :\n{}", e, response).to_rsk_err()),
Ok(v) => v
};
let mut game_data = game_data.lock()?;
*game_data = Some(received_game_data);
}
}
pub async fn goto_process(client: Client, team:Team, number:RobotNumber) -> RskResult<()> {
loop {
let destination = client.get_destination(team, number).await?;
let actual_pose = client.get_robot(team, number)?.get_pose()?;
if let Some(destination) = destination {
if let Err(e) = client.cmd_sender.send((team, number, destination.to_control_command(actual_pose))) {
println!("Client.goto_process() : error while sending command : {:?}", e);
}
}
tokio::time::sleep(Duration::from_millis(50)).await;
}
}
pub async fn get_destinations(&self) -> RskResult<Destinations> {
let destinations = (*self.destinations.lock().await).clone();
Ok(destinations)
}
pub fn get_game_data(&self) -> RskResult<GameData> {
let gd = (*self.game_data.lock()?).clone();
match gd {
Some(gd) => Ok(gd),
None => Err("Client.get_game_data() : no game data available".to_rsk_err())
}
}
pub fn get_robot(&self, team:Team, number:RobotNumber) -> RskResult<Robot> {
let gd = self.get_game_data()?;
let markers_robot = match team {
Team::Blue => {
match number {
RobotNumber::One => gd.markers.blue1,
RobotNumber::Two => gd.markers.blue2
}
},
Team::Green => {
match number {
RobotNumber::One => gd.markers.green1,
RobotNumber::Two => gd.markers.green2
}
}
};
let referee_robot = match team {
Team::Blue => {
match number {
RobotNumber::One => gd.referee.teams.blue.robots.one,
RobotNumber::Two => gd.referee.teams.blue.robots.two
}
},
Team::Green => {
match number {
RobotNumber::One => gd.referee.teams.green.robots.one,
RobotNumber::Two => gd.referee.teams.green.robots.two
}
}
};
let leds = match team {
Team::Blue => {
match number {
RobotNumber::One => gd.leds.blue1,
RobotNumber::Two => gd.leds.blue2
}
},
Team::Green => {
match number {
RobotNumber::One => gd.leds.green1,
RobotNumber::Two => gd.leds.green2
}
}
};
Ok(Robot {
team,
number,
position: markers_robot.position,
orientation: markers_robot.orientation,
leds,
penalized: referee_robot.penalized,
penalized_remaining: referee_robot.penalized_remaining,
penalized_reason: referee_robot.penalized_reason,
preempted: referee_robot.preempted,
preemption_reasons: referee_robot.preemption_reasons,
client: &self
})
}
pub async fn is_arrived(&self, team:Team, number:RobotNumber) -> RskResult<bool> {
let robot = self.get_robot(team, number)?;
let destination = self.get_destination(team, number).await?;
if let Some(destination) = destination {
return Ok((robot.get_cache_pose() - destination.pose).norm() < 0.03 && (robot.get_cache_pose().get_theta() - destination.pose.get_theta()).abs() < 0.05);
}
Ok(true)
}
pub async fn get_destination(&self, team:Team, number:RobotNumber) -> RskResult<Option<Destination>> {
let destinations = self.destinations.lock().await;
let destination = match team {
Team::Blue => {
match number {
RobotNumber::One => destinations.blue1,
RobotNumber::Two => destinations.blue2
}
},
Team::Green => {
match number {
RobotNumber::One => destinations.green1,
RobotNumber::Two => destinations.green2
}
}
};
Ok(destination)
}
pub async fn set_destination(&self, team:Team, number:RobotNumber, destination:Option<Destination>) -> RskResult<()> {
let mut destinations = self.destinations.lock().await;
match team {
Team::Blue => {
match number {
RobotNumber::One => destinations.blue1 = destination,
RobotNumber::Two => destinations.blue2 = destination
}
},
Team::Green => {
match number {
RobotNumber::One => destinations.green1 = destination,
RobotNumber::Two => destinations.green2 = destination
}
}
};
Ok(())
}
pub async fn goto(&self, team:Team, number:RobotNumber, destination:Destination) -> RskResult<()> {
self.set_destination(team, number, Some(destination)).await?;
Ok(())
}
pub async fn stop(&self, team:Team, number:RobotNumber) -> RskResult<()> {
self.set_destination(team, number, None).await?;
Ok(())
}
pub async fn goto_blocking(&self, team:Team, number:RobotNumber, destination:Destination) -> RskResult<()> {
loop {
self.goto(team, number, destination).await?;
if self.is_arrived(team, number).await? {
break;
}
tokio::time::sleep(Duration::from_millis(10)).await;
}
Ok(())
}
pub fn get_ball(&self) -> RskResult<Position> {
Ok(self.get_game_data()?.ball)
}
pub async fn get_allies(&self) -> RskResult<(Robot, Robot)> {
Ok((self.get_robot(self.team, RobotNumber::One)?, self.get_robot(self.team, RobotNumber::Two)?))
}
}