use crate::object::robot::Robot;
use crate::object::sensors::LiDARMsg;
use crate::object::static_obj::StaticObj;
use crate::object::wall::Wall;
use crate::object::OccupancyGrid;
use crate::object::Trolley;
use crate::parameter::*;
use crate::parser::*;
use crate::prelude::traits::{Collidable, Drawable, Genericbject, GuiObject};
use crate::prelude::Footprint;
use crate::utils::interpolate_pose;
use futures::executor::block_on;
use macroquad::prelude::*;
#[derive(Debug, Clone, Copy)]
pub struct RobotHandler {
pub id: usize,
}
impl RobotHandler {
pub fn new(id: usize) -> RobotHandler {
RobotHandler { id }
}
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub enum SelectedObjectType {
Robot,
Other,
}
#[derive(Debug, Clone, Copy, PartialEq, PartialOrd)]
pub enum ObjectParameterType {
Position(f32, f32),
Rotation(f32),
Bounds(f32, f32),
}
pub struct FullInformation {
pub id: String,
pub pose: (f32, f32, f32),
pub velocity: (f32, f32, f32),
pub bounds: (f32, f32),
pub drive_type: String,
}
impl Default for FullInformation {
fn default() -> Self {
Self {
id: "ID".to_string(),
pose: (0.0, 0.0, 0.0),
velocity: (0.0, 0.0, 0.0),
bounds: (0.0, 0.0),
drive_type: "NA".to_string(),
}
}
}
pub struct SimulationHandler {
robots: Vec<Robot>,
objects: Vec<Box<dyn Genericbject>>,
walls: Vec<Wall>,
static_objects: Vec<StaticObj>,
filepath: String,
occupancy_grid: Option<OccupancyGrid>,
}
impl SimulationHandler {
pub fn new() -> SimulationHandler {
return SimulationHandler {
robots: Vec::new(),
objects: Vec::new(),
walls: Vec::new(),
static_objects: Vec::new(),
filepath: "".to_string(),
occupancy_grid: None,
};
}
pub fn from_file(filepath: String) -> (SimulationHandler, Vec<(String, RobotHandler)>) {
let file_path_copy = filepath.clone();
let config = get_config_from_file(filepath);
let mut sim_handle = SimulationHandler::new();
sim_handle.load_file_path(file_path_copy.to_owned());
let mut robot_handles = Vec::new();
match config {
Some(config) => {
for robot in config.robots.iter() {
let mut trolley = None;
if let Some(conf) = &robot.attached_config {
trolley = Some(Trolley::new(
"trolley".to_string(),
&(conf.length, conf.width),
));
}
let handle = sim_handle.add_robot(Robot::new(
robot.id.clone(),
robot.pose,
robot.vel,
robot.lidar,
robot.footprint.clone(),
robot.drive_type,
robot.add_noise,
trolley,
));
robot_handles.push(handle);
}
for wall in config.walls.iter() {
sim_handle.add_wall(Wall::new(wall.endpoints.clone()));
}
for obj in config.static_objects.iter() {
sim_handle.add_static_obj(StaticObj::new(
obj.center,
obj.width,
obj.height,
obj.rotation,
));
}
}
None => {}
}
return (sim_handle, robot_handles);
}
pub fn load_file_path(&mut self, path: String) {
self.filepath = path.clone();
}
pub fn reset(&mut self) -> Vec<(String, RobotHandler)> {
self.robots.clear();
self.objects.clear();
self.walls.clear();
self.static_objects.clear();
self.occupancy_grid = None;
let config_return = get_config_from_file(self.filepath.to_owned());
match config_return {
Some(config) => {
let mut all_robot_handlers: Vec<(String, RobotHandler)> = Vec::new();
for robot in config.robots.iter() {
let mut trolley = None;
if let Some(conf) = &robot.attached_config {
trolley = Some(Trolley::new(
"trolley".to_string(),
&(conf.length, conf.width),
));
}
let handle = self.add_robot(Robot::new(
robot.id.clone(),
robot.pose,
robot.vel,
robot.lidar,
robot.footprint.clone(),
robot.drive_type,
robot.add_noise,
trolley,
));
all_robot_handlers.push(handle);
}
for wall in config.walls.iter() {
self.add_wall(Wall::new(wall.endpoints.clone()));
}
for obj in config.static_objects.iter() {
self.add_static_obj(StaticObj::new(
obj.center,
obj.width,
obj.height,
obj.rotation,
));
}
if let Some(occupancy_grid_config) = config.occupancy_grid {
let occ = block_on(OccupancyGrid::new(&occupancy_grid_config.yaml_file_path))
.unwrap();
self.occupancy_grid = Some(occ);
}
return all_robot_handlers;
}
None => {
return Vec::new();
}
}
}
pub fn add_robot(&mut self, robot: Robot) -> (String, RobotHandler) {
let name = robot.id.clone();
println!(
"
Added robot with name : {}. Radius: {}, Center: {}, {}",
name,
robot.get_bounds().0,
robot.get_center().0,
robot.get_center().1
);
self.robots.push(robot);
return (
name,
RobotHandler {
id: self.robots.len() - 1,
},
);
}
pub async fn add_occupancy_grid(&mut self, yaml_file_path: &str) {
let occ = OccupancyGrid::new(yaml_file_path).await;
match occ {
Ok(occ) => {
self.occupancy_grid = Some(occ);
println!("Added OccupancyGrid");
}
Err(e) => {
println!("Could not load occupancy grid: {}", e)
}
}
}
pub fn add_wall(&mut self, wall: Wall) {
self.objects.push(Box::new(wall.clone()));
println!("\n{:?}\n added", wall.coords);
self.walls.push(wall.clone());
}
pub fn add_static_obj(&mut self, obj: StaticObj) {
self.objects.push(Box::new(obj.clone()));
self.static_objects.push(obj.clone());
}
pub fn control(&mut self, robot: &RobotHandler, control: (f32, f32, f32)) {
self.robots[robot.id].control(control);
}
pub fn sense(&self, robot: &RobotHandler) -> LiDARMsg {
let mut collidables_vector = Vec::new();
for obj in self.objects.iter() {
collidables_vector.push(obj.get_collidable());
}
for obj in self.robots.iter() {
collidables_vector.push(obj.get_collidable());
}
match &self.occupancy_grid {
Some(occ) => {
collidables_vector.push(Box::new(occ.clone()));
}
None => {}
}
return self.robots[robot.id].sense(&collidables_vector);
}
pub fn get_pose(&self, robot: &RobotHandler) -> (f32, f32, f32) {
return self.robots[robot.id].pose;
}
pub fn get_nearest_object(&self, x: f32, y: f32) -> (Option<SelectedObjectType>, i32) {
let mut selected_object_type = None;
let mut nearest_index: i32 = -1;
for i in 0..self.robots.len() {
let robot = self.robots[i].clone();
let (rx, ry) = (robot.pose.0, robot.pose.1);
match robot.shape {
Footprint::Circular(r) => {
if (rx - x).abs() < r.radius && (ry - y).abs() < r.radius {
selected_object_type = Some(SelectedObjectType::Robot);
nearest_index = i as i32;
return (selected_object_type, nearest_index);
}
}
Footprint::Rectangular(c) => {
if (rx - x).abs() < c.half_extents.x && (ry - y).abs() < c.half_extents.y {
selected_object_type = Some(SelectedObjectType::Robot);
nearest_index = i as i32;
return (selected_object_type, nearest_index);
}
}
}
}
for i in 0..self.objects.len() {
let object = self.objects[i].as_ref();
if (x - object.get_pose().0).abs() < object.get_bounds().0
&& (y - object.get_pose().1).abs() < object.get_bounds().1
{
selected_object_type = Some(SelectedObjectType::Other);
nearest_index = i as i32;
return (selected_object_type, nearest_index);
}
}
return (selected_object_type, nearest_index);
}
pub fn get_parameters_of_selected_object(
&self,
selected_object: (Option<SelectedObjectType>, i32),
parameter_type: ObjectParameterType,
) -> ObjectParameterType {
let (selected_type, index) = selected_object;
match selected_type {
Some(t) => match t {
SelectedObjectType::Robot => match parameter_type {
ObjectParameterType::Bounds(_w, _h) => {
let bounds = self.robots[index as usize].get_bounds();
return ObjectParameterType::Bounds(bounds.0, bounds.1);
}
ObjectParameterType::Rotation(_angle) => {
let angle = self.robots[index as usize].get_rotation();
return ObjectParameterType::Rotation(angle);
}
ObjectParameterType::Position(_x, _y) => {
let position = self.robots[index as usize].get_center();
return ObjectParameterType::Position(position.0, position.1);
}
},
SelectedObjectType::Other => match parameter_type {
ObjectParameterType::Bounds(_w, _h) => {
let bounds = self.objects[index as usize].get_bounds();
return ObjectParameterType::Bounds(bounds.0, bounds.1);
}
ObjectParameterType::Rotation(_angle) => {
let angle = self.objects[index as usize].get_rotation();
return ObjectParameterType::Rotation(angle);
}
ObjectParameterType::Position(_x, _y) => {
let position = self.objects[index as usize].get_center();
return ObjectParameterType::Position(position.0, position.1);
}
},
},
None => {
return ObjectParameterType::Rotation(0.0);
}
}
}
pub fn get_full_information_of_selected_object(
&self,
selected_object: (Option<SelectedObjectType>, i32),
) -> FullInformation {
let (selected_type, index) = selected_object;
match selected_type {
Some(obj) => match obj {
SelectedObjectType::Robot => {
if !self.robots.is_empty() {
let robot = self.robots[index as usize].clone();
let bounds = robot.get_bounds();
return FullInformation {
id: robot.id,
pose: robot.pose,
velocity: robot.vel,
bounds: bounds,
drive_type: robot.drive_type.to_string(),
};
} else {
return FullInformation::default();
}
}
SelectedObjectType::Other => {
let obj = &self.objects[index as usize];
return FullInformation {
id: "Object".to_string(),
pose: obj.get_pose(),
velocity: (0.0, 0.0, 0.0),
bounds: obj.get_bounds(),
drive_type: "NA".to_string(),
};
}
},
None => {
return FullInformation::default();
}
}
}
pub fn change_parameters_of_selected_object(
&mut self,
selected_object: (Option<SelectedObjectType>, i32),
parameter_type: ObjectParameterType,
) {
let (selected_type, index) = selected_object;
match selected_type {
Some(t) => match t {
SelectedObjectType::Robot => match parameter_type {
ObjectParameterType::Bounds(w, h) => {
self.robots[index as usize].modify_bounds(w, h)
}
ObjectParameterType::Rotation(angle) => {
self.robots[index as usize].modify_rotation(angle)
}
ObjectParameterType::Position(x, y) => {
self.robots[index as usize].modify_position(x, y)
}
},
SelectedObjectType::Other => match parameter_type {
ObjectParameterType::Bounds(w, h) => {
self.objects[index as usize].modify_bounds(w, h)
}
ObjectParameterType::Rotation(angle) => {
self.objects[index as usize].modify_rotation(angle)
}
ObjectParameterType::Position(x, y) => {
self.objects[index as usize].modify_position(x, y)
}
},
},
None => {}
}
}
pub fn delete_selected_object(&mut self, selected_object: (Option<SelectedObjectType>, i32)) {
let (selected_type, index) = selected_object;
match selected_type {
Some(t) => match t {
SelectedObjectType::Other => {
let _val = self.objects.remove(index as usize);
}
SelectedObjectType::Robot => {
let _val = self.robots.remove(index as usize);
}
},
None => {}
}
}
pub fn step(&mut self) {
let mut next_poses: Vec<(f32, f32, f32)> = Vec::with_capacity(self.robots.len());
for robot in &mut self.robots {
let next_pose = robot.next();
next_poses.push(next_pose);
}
let mut collisions: Vec<Option<f32>> = vec![None; self.robots.len()];
for i in 0..self.robots.len() {
let robot = &self.robots[i];
let start_pose = robot.get_pose();
let end_pose = next_poses[i];
for object in &self.objects {
if let Some(toi) = robot.collision_check_at_toi(
&*object.get_collidable(),
&start_pose,
&end_pose,
None,
None,
) {
collisions[i] = Some(collisions[i].map_or(toi, |t| t.min(toi)));
}
}
if let Some(occ) = &self.occupancy_grid {
if let Some(toi) = occ.collision_check_at_toi(
&*robot.get_collidable(),
&start_pose,
&end_pose,
None,
None,
) {
collisions[i] = Some(collisions[i].map_or(toi, |t| t.min(toi)));
}
}
for j in 0..self.robots.len() {
if i != j {
let robot2 = &self.robots[j];
let start_pose2 = robot2.get_pose();
let end_pose2 = next_poses[j];
if let Some(toi) = robot.collision_check_at_toi(
robot2,
&start_pose,
&end_pose,
Some(start_pose2),
Some(end_pose2),
) {
collisions[i] = Some(collisions[i].map_or(toi, |t| t.min(toi)));
collisions[j] = Some(collisions[j].map_or(toi, |t| t.min(toi)));
}
}
}
}
for i in 0..self.robots.len() {
let robot = &mut self.robots[i];
let start_pose = robot.get_pose();
let end_pose = next_poses[i];
if let Some(toi) = collisions[i] {
let collision_pose = interpolate_pose(&start_pose, &end_pose, toi);
robot.step(&collision_pose);
robot.control((0.0, 0.0, 0.0));
} else {
robot.step(&end_pose);
}
}
}
pub fn to_config(&self) -> Config {
let mut robot_config_vectors: Vec<RobotConfig> = Vec::new();
for robot in self.robots.iter() {
robot_config_vectors.push(robot.into_config());
}
let mut wall_config_vector: Vec<WallConfig> = Vec::new();
for wall in self.walls.iter() {
wall_config_vector.push(WallConfig {
endpoints: wall.coords.clone(),
});
}
let mut static_objects_config: Vec<StaticObjConfig> = Vec::new();
for obj in self.static_objects.iter() {
static_objects_config.push(StaticObjConfig {
center: obj.center.clone(),
width: obj.width,
height: obj.height,
rotation: obj.rotation,
});
}
let mut occ_config = None;
if let Some(occ) = &self.occupancy_grid {
occ_config = Some(OccupancyGridConfig {
yaml_file_path: occ.yaml_file_path.to_string(),
});
}
Config {
robots: robot_config_vectors,
walls: wall_config_vector,
static_objects: static_objects_config,
occupancy_grid: occ_config,
}
}
pub fn draw_lines(&self) {
let x_lims = (-25.0, 25.0);
let y_lims = (-25.0, 25.0);
let increment = 1.0;
let mut x = x_lims.0;
let mut y = y_lims.0;
while x <= x_lims.1 {
let init_coord = Self::tf_function((x, y_lims.0));
let final_coord = Self::tf_function((x, y_lims.1));
draw_line(
init_coord.0,
init_coord.1,
final_coord.0,
final_coord.1,
1.0,
LIGHTGRAY,
);
x += increment;
}
while y <= x_lims.1 {
let init_coord = Self::tf_function((x_lims.0, y));
let final_coord = Self::tf_function((x_lims.1, y));
draw_line(
init_coord.0,
init_coord.1,
final_coord.0,
final_coord.1,
1.0,
LIGHTGRAY,
);
y += increment;
}
let origin_in_pixel = Self::tf_function((0.0, 0.0));
let one_meter_x = Self::tf_function((1.0, 0.0));
let one_meter_y = Self::tf_function((0.0, 1.0));
draw_line(
origin_in_pixel.0,
origin_in_pixel.1,
one_meter_x.0,
one_meter_x.1,
2.0,
RED,
);
draw_line(
origin_in_pixel.0,
origin_in_pixel.1,
one_meter_y.0,
one_meter_y.1,
2.0,
GREEN,
);
}
pub fn draw_occ_grid(&self) {
match &self.occupancy_grid {
Some(occ) => occ.draw(Self::tf_function),
None => {}
}
}
pub fn draw(&self) {
for robot in self.robots.iter() {
robot.draw(Self::tf_function);
}
for object in self.objects.iter() {
object.draw(Self::tf_function);
}
}
pub fn draw_bounds_of_selected_object(
&self,
selected_object: (Option<SelectedObjectType>, i32),
) {
let (selected_object_type, index) = selected_object;
match selected_object_type {
Some(object) => match object {
SelectedObjectType::Robot => {
self.robots[index as usize].draw_bounds(Self::tf_function);
}
SelectedObjectType::Other => {
self.objects[index as usize].draw_bounds(Self::tf_function);
}
},
None => {}
}
}
pub fn tf_function(pos: (f32, f32)) -> (f32, f32) {
let i = (pos.0 - XLIMS.0) / RESOLUTION;
let j = (YLIMS.1 - pos.1) / RESOLUTION;
return (i, j);
}
pub fn get_world_from_pixel(px: f32, py: f32) -> (f32, f32) {
let wx = XLIMS.0 + px * RESOLUTION;
let wy = YLIMS.1 - py * RESOLUTION;
return (wx, wy);
}
pub fn scale_function(value: f32) -> f32 {
return value * RESOLUTION;
}
pub fn inverse_scale_function(value: f32) -> f32 {
return value / RESOLUTION;
}
}
unsafe impl Sync for SimulationHandler {}
unsafe impl Send for SimulationHandler {}