From 5f694b0c7568fa602eaed0676b26cb040b7904b1 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Sat, 15 Feb 2025 23:28:02 -0500 Subject: [PATCH 1/2] Don't have generator modify trajectory parameters while generating --- src-core/src/generation/generate.rs | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src-core/src/generation/generate.rs b/src-core/src/generation/generate.rs index 8fdd98495e..b72fd19797 100644 --- a/src-core/src/generation/generate.rs +++ b/src-core/src/generation/generate.rs @@ -115,6 +115,9 @@ pub fn generate( mut trajectory_file: TrajectoryFile, handle: i64, ) -> ChoreoResult { + let mut original = trajectory_file.clone(); + + // these two functions can make changes to the trajectory file set_initial_guess(&mut trajectory_file); adjust_headings(&mut trajectory_file)?; @@ -125,5 +128,11 @@ pub fn generate( gen.add_omni_transformer::(); gen.add_omni_transformer::(); - gen.generate() + let new_traj = gen.generate(); + if new_traj.is_ok() { + original.trajectory = new_traj?.trajectory; + Ok(original) + } else { + new_traj + } } From 652f889c5ac0aa5e3cbbb1c96415c29978a9055d Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Tue, 25 Feb 2025 12:39:28 -0500 Subject: [PATCH 2/2] Major change in strategy; All changes are now located in the CLI --- src-cli/src/main.rs | 114 +++++++++++++++++++--------- src-core/src/generation/generate.rs | 11 +-- 2 files changed, 78 insertions(+), 47 deletions(-) diff --git a/src-cli/src/main.rs b/src-cli/src/main.rs index 80c4be3e97..5b6dd2a5db 100644 --- a/src-cli/src/main.rs +++ b/src-cli/src/main.rs @@ -5,6 +5,7 @@ use std::{ thread::{self, JoinHandle}, }; +use choreo_core::spec::trajectory::TrajectoryFile; use choreo_core::{ file_management::{self, WritingResources}, generation::generate::generate, @@ -192,48 +193,87 @@ impl Cli { let cln_project = project.clone(); let cln_resources = resources.clone(); let cln_trajectory_name = trajectory_name.clone(); - let handle = - thread::spawn( - move || match generate(cln_project.clone(), trajectory, i as i64) { - Ok(new_trajectory) => { - let runtime = - choreo_core::tokio::runtime::Builder::new_current_thread() - .enable_all() - .build() - .expect("Failed to build tokio runtime"); - let write_result = runtime.block_on( - file_management::write_trajectory_file_immediately( - &cln_resources, - new_trajectory, - ), + let handle = thread::spawn(move || { + match generate(cln_project.clone(), trajectory.clone(), i as i64) { + Ok(regenerated_trajectory) => { + // The raw generation has the opportunity to change waypoints to help massage the trajoptlib generation. We only care about adopting the generated trajectory + let mut sanitized_regenerated_trajectory = TrajectoryFile { + trajectory: regenerated_trajectory.trajectory, + ..trajectory + }; + + // The initial generation does not update the control intervals. This runs an initial guess pass for each waypoint on the sanitized version of the file in-place. + let control_intervals = + choreo_core::generation::intervals::guess_control_interval_counts( + &cln_project.config.snapshot(), + &sanitized_regenerated_trajectory.params.snapshot(), ); - match write_result { - Ok(_) => { - tracing::info!( - "Successfully generated trajectory {:} for {:}", - cln_trajectory_name, - cln_project.name - ); - } - Err(e) => { - tracing::error!( - "Failed to write trajectory {:} for {:}: {:}", - cln_trajectory_name, - cln_project.name, - e - ); + match control_intervals { + Ok(control_intervals) => { + for (i, count) in control_intervals.iter().enumerate() { + let waypoint = + &mut sanitized_regenerated_trajectory.params.waypoints[i]; + if waypoint.override_intervals && *count != waypoint.intervals { + tracing::warn!("Control interval guessing did not ignore override intervals!"); + } else { + waypoint.intervals = *count; + } } } + Err(e) => { + tracing::error!( + "Guess intervals failed {:} for {:}: {:}", + cln_trajectory_name, + cln_project.name, + e + ); + } } - Err(e) => { - tracing::error!( - "Failed to generate trajectory {:}: {:}", - cln_trajectory_name, - e - ); + + let runtime = choreo_core::tokio::runtime::Builder::new_current_thread() + .enable_all() + .build() + .expect("Failed to build tokio runtime"); + + // Finally, we write the fully generated file to disk + let write_result = + runtime.block_on(file_management::write_trajectory_file_immediately( + &cln_resources, + TrajectoryFile { + trajectory: sanitized_regenerated_trajectory.trajectory, + snapshot: Some( + sanitized_regenerated_trajectory.params.snapshot(), + ), + ..sanitized_regenerated_trajectory + }, + )); + match write_result { + Ok(_) => { + tracing::info!( + "Successfully generated trajectory {:} for {:}", + cln_trajectory_name, + cln_project.name + ); + } + Err(e) => { + tracing::error!( + "Failed to write trajectory {:} for {:}: {:}", + cln_trajectory_name, + cln_project.name, + e + ); + } } - }, - ); + } + Err(e) => { + tracing::error!( + "Failed to generate trajectory {:}: {:}", + cln_trajectory_name, + e + ); + } + } + }); thread_handles.push(handle); } diff --git a/src-core/src/generation/generate.rs b/src-core/src/generation/generate.rs index b72fd19797..8fdd98495e 100644 --- a/src-core/src/generation/generate.rs +++ b/src-core/src/generation/generate.rs @@ -115,9 +115,6 @@ pub fn generate( mut trajectory_file: TrajectoryFile, handle: i64, ) -> ChoreoResult { - let mut original = trajectory_file.clone(); - - // these two functions can make changes to the trajectory file set_initial_guess(&mut trajectory_file); adjust_headings(&mut trajectory_file)?; @@ -128,11 +125,5 @@ pub fn generate( gen.add_omni_transformer::(); gen.add_omni_transformer::(); - let new_traj = gen.generate(); - if new_traj.is_ok() { - original.trajectory = new_traj?.trajectory; - Ok(original) - } else { - new_traj - } + gen.generate() }