agent_tk/simulation/
tst.rs

1//! TST Simulation
2
3use futures::future::FutureExt;
4use geo::{Area, Centroid};
5
6use crate::definitions::agent as definitions_agent;
7use crate::definitions::tst as definitions_tst;
8use crate::definitions::tst::SpeedCompute;
9use crate::execution::tst as execution_tst;
10use crate::execution::tst::TreeExecutorTrait;
11use crate::{Error, Result};
12
13mod utils;
14
15//  ____  _                 _       _   _             ____                 _ _
16// / ___|(_)_ __ ___  _   _| | __ _| |_(_) ___  _ __ |  _ \ ___  ___ _   _| | |_
17// \___ \| | '_ ` _ \| | | | |/ _` | __| |/ _ \| '_ \| |_) / _ \/ __| | | | | __|
18//  ___) | | | | | | | |_| | | (_| | |_| | (_) | | | |  _ |  __/\__ \ |_| | | |_
19// |____/|_|_| |_| |_|\__,_|_|\__,_|\__|_|\___/|_| |_|_| \_\___||___/\__,_|_|\__|
20
21/// Result of the simulation, with the estimated cost and final states.
22#[derive(Default, Clone, serde::Serialize, serde::Deserialize)]
23pub struct SimulationResult
24{
25  estimated_cost: f32,
26  final_states: crate::states::States,
27}
28
29impl SimulationResult
30{
31  /// Return the estimated cost
32  pub fn get_estimated_cost(&self) -> f32
33  {
34    self.estimated_cost
35  }
36  /// Get the final states, after executing the TST
37  pub fn get_final_states(&self) -> &crate::states::States
38  {
39    &self.final_states
40  }
41}
42
43//  ____  _                 _       _   _             _____                     _   _
44// / ___|(_)_ __ ___  _   _| | __ _| |_(_) ___  _ __ | ____|_  _____  ___ _   _| |_(_) ___  _ __
45// \___ \| | '_ ` _ \| | | | |/ _` | __| |/ _ \| '_ \|  _| \ \/ / _ \/ __| | | | __| |/ _ \| '_ \
46//  ___) | | | | | | | |_| | | (_| | |_| | (_) | | | | |___ >  <  __/ (__| |_| | |_| | (_) | | | |
47// |____/|_|_| |_| |_|\__,_|_|\__,_|\__|_|\___/|_| |_|_____/_/\_\___|\___|\__,_|\__|_|\___/|_| |_|
48
49/// Simulate the execution of a task, to compute the feasibility and the associated cost.
50pub async fn simulate_execution(
51  initial_states: crate::states::States,
52  capabilities: definitions_agent::capabilities::Capabilities,
53  tst_node: definitions_tst::Node,
54) -> Result<SimulationResult>
55{
56  let agent = Agent {
57    capabilities,
58    ..Default::default()
59  };
60  agent.simulation_result.lock().unwrap().final_states = initial_states;
61  let executor =
62    execution_tst::DefaultTreeExecutor::<MoveToExecutor, SearchAreaExecutor>::from_agent(&agent);
63  let r = executor.execute(tst_node);
64  let res = r.await;
65  if let Err(e) = res
66  {
67    Err(e)
68  }
69  else
70  {
71    Ok(agent.simulation_result.lock().unwrap().clone())
72  }
73}
74
75// Simulation executor
76
77// Structures
78
79#[derive(Clone, Default)]
80struct Agent
81{
82  capabilities: definitions_agent::capabilities::Capabilities,
83  simulation_result: std::sync::Arc<std::sync::Mutex<SimulationResult>>,
84}
85
86impl execution_tst::Agent for Agent {}
87
88struct MoveToExecutor
89{
90  agent: Agent,
91}
92
93impl execution_tst::CreatableFromAgent<Agent, MoveToExecutor> for MoveToExecutor
94{
95  fn from_agent(agent: &Agent) -> MoveToExecutor
96  {
97    MoveToExecutor {
98      agent: agent.clone(),
99    }
100  }
101}
102
103impl execution_tst::Executor<definitions_tst::MoveTo> for MoveToExecutor
104{
105  fn execute<'b>(
106    &self,
107    _executors: &'b impl execution_tst::Executors,
108    t: definitions_tst::MoveTo,
109  ) -> execution_tst::ExecutionResult<'b>
110  {
111    let simulation_result = self.agent.simulation_result.clone();
112    let capabilities = self.agent.capabilities.clone();
113    async move {
114      let mut simulation_result = simulation_result.lock().unwrap();
115      let current_position = simulation_result.final_states.get_position()?;
116
117      let move_to_capability = capabilities
118        .get_move()
119        .map_err(|e| crate::Error::ExecutionFailed(Box::new(e)))?;
120      let vel = t
121        .params
122        .speed
123        .compute_velocity(move_to_capability.get_max_velocity());
124      simulation_result.estimated_cost += utils::haversine_distance(
125        current_position.longitude,
126        current_position.latitude,
127        t.params.waypoint.longitude,
128        t.params.waypoint.latitude,
129      ) as f32
130        * vel;
131
132      simulation_result
133        .final_states
134        .update_state(crate::states::Position {
135          latitude: t.params.waypoint.latitude,
136          longitude: t.params.waypoint.longitude,
137        })?;
138      Ok::<(), Error>(())
139    }
140    .boxed()
141  }
142}
143
144struct SearchAreaExecutor
145{
146  agent: Agent,
147}
148
149impl execution_tst::CreatableFromAgent<Agent, SearchAreaExecutor> for SearchAreaExecutor
150{
151  fn from_agent(agent: &Agent) -> SearchAreaExecutor
152  {
153    SearchAreaExecutor {
154      agent: agent.clone(),
155    }
156  }
157}
158
159impl execution_tst::Executor<definitions_tst::SearchArea> for SearchAreaExecutor
160{
161  fn execute<'b>(
162    &self,
163    _executors: &'b impl execution_tst::Executors,
164    t: definitions_tst::SearchArea,
165  ) -> execution_tst::ExecutionResult<'b>
166  {
167    let simulation_result = self.agent.simulation_result.clone();
168    let capabilities = self.agent.capabilities.clone();
169
170    async move {
171      let mut simulation_result = simulation_result.lock().unwrap();
172      let current_position = simulation_result.final_states.get_position()?;
173
174      let move_to_capability = capabilities
175        .get_move()
176        .map_err(|e| crate::Error::ExecutionFailed(Box::new(e)))?;
177
178      // Compute surface of polygon and center
179      let mut polygon_points = Vec::<(f64, f64)>::new();
180
181      for pt in t.params.area
182      {
183        polygon_points.push((pt.longitude, pt.latitude));
184      }
185
186      polygon_points.push(polygon_points[0]);
187
188      let polygon = geo::Polygon::new(geo::LineString::from(polygon_points), vec![]);
189
190      let center = polygon.centroid().unwrap();
191      simulation_result.estimated_cost += utils::haversine_distance(
192        current_position.longitude,
193        current_position.latitude,
194        center.x(),
195        center.y(),
196      ) as f32;
197      simulation_result.estimated_cost +=
198        polygon.unsigned_area() as f32 * move_to_capability.get_max_velocity();
199
200      simulation_result
201        .final_states
202        .update_state(crate::states::Position {
203          longitude: center.x(),
204          latitude: center.y(),
205        })?;
206      Ok::<(), Error>(())
207    }
208    .boxed()
209  }
210}
agent_tk/simulation/tst.rs

agent_tk/simulation/
tst.rs
