Improved multithreading on Cartesian planner.

Cargo.toml

@@ -49,15 +49,21 @@ bevy_egui = { version = "0.24", optional = true }
 
 # Needed for stroke planning
 bitflags = "2.6.0"
-# Copyright 2017 Takashi Ogura Apache 2 license.
-rrt = { version = "0.7.0", optional = true }
+
+#rrt = { version = "0.7.0", optional = true }
+# Copyright 2017 Takashi Ogura Apache 2 license. This is not integrated in the code (src/path_plan/rrt_to.rs)
+# as we needed changes there.
+# Dependencies for this code:
+kdtree = { version = "0.7", optional = true }
+num-traits = { version = "0.2", optional = true }
+tracing = { version = "0.1", optional = true }
 
 [features]
 default = ["allow_filesystem", "collisions", "stroke_planning", "visualization"]
 allow_filesystem = ["yaml-rust2", "sxd-document", "regex", "clap", "ply-rs-bw", "stl_io"]
 collisions = ["parry3d", "rayon"]
 visualization = ["bevy", "bevy_egui", "collisions", "allow_filesystem"]
-stroke_planning = ["rrt", "collisions"]
+stroke_planning = ["kdtree", "num-traits", "tracing", "collisions"]
 
 # To disable filesystem, collision and visualizatio support:
 # rs-opw-kinematics = { version = "1.8.3", default-features = false }

src/lib.rs

@@ -105,6 +105,11 @@ mod camera_controller;
 #[path = "path_plan/rrt.rs"]
 pub mod rrt;
 
+#[cfg(feature = "stroke_planning")]
+#[path = "path_plan/rrt_to.rs"]
+mod rrt_to;
+
+
 #[cfg(test)]
 #[cfg(feature = "allow_filesystem")]
 mod tests;

src/path_plan/cartesian.rs

@@ -9,6 +9,8 @@ use bitflags::bitflags;
 use nalgebra::Translation3;
 use rayon::prelude::{IntoParallelRefIterator, ParallelIterator};
 use std::fmt;
+use std::sync::Arc;
+use std::sync::atomic::{AtomicBool, Ordering};
 
 /// Reasonable default transition costs. Rotation of smaller joints is more tolerable.
 /// The sum of all weights is 6.0
@@ -64,18 +66,18 @@ bitflags! {
         const TRACE =               0b00000100;
         /// Position is linear interpolation between two poses of the trace. These poses
         /// are not needed for the robots that have built-in support for Cartesian stroke,
-        /// but may be important for more developed models that only rotate between 
+        /// but may be important for more developed models that only rotate between
         /// the given joint positions without guarantees that TCP movement is linear.
         const LIN_INTERP = 0b00001000;
         /// Position corresponds the starting pose ("land") that is normally little above
         /// the start of the required trace
         const LAND =                0b00010000;
         /// Position corresponds the ennding pose ("park") that is normally little above
-        /// the end of the required trace. This is the last pose to include into the 
+        /// the end of the required trace. This is the last pose to include into the
         /// output.
-        const PARK =                0b00100000;   
+        const PARK =                0b00100000;
         /// Combined flag representing the "original" position, so the one that was
-        /// given in the input. 
+        /// given in the input.
         const ORIGINAL = Self::TRACE.bits() | Self::LAND.bits() | Self::PARK.bits();
     }
 }
@@ -118,7 +120,7 @@ fn flag_representation(flags: &PathFlags) -> String {
         (PathFlags::PARK, "PARK"),
         (PathFlags::TRACE, "TRACE"),
         (PathFlags::CARTESIAN, "CARTESIAN"),
-        (PathFlags::ONBOARDING, "ONBOARDING"),        
+        (PathFlags::ONBOARDING, "ONBOARDING"),
     ];
 
     FLAG_MAP
@@ -194,19 +196,28 @@ impl Cartesian<'_> {
         }
         let poses = self.with_intermediate_poses(land, &steps, park);
         println!("Probing {} strategies", strategies.len());
+
+        let stop = Arc::new(AtomicBool::new(false));
+
         strategies
             .par_iter()
-            .find_map_any(
-                |strategy| match self.probe_strategy(from, strategy, &poses) {
-                    Ok(outcome) => Some(Ok(outcome)), // Return success wrapped in Some
+            .find_map_any(|strategy| {
+                match self.probe_strategy(
+                    from, strategy, &poses, &stop
+                ) {
+                    Ok(outcome) => {
+                        println!("Strategy worked out: {:?}", strategy);
+                        stop.store(true, Ordering::Relaxed);
+                        Some(Ok(outcome))
+                    }
                     Err(msg) => {
                         if self.debug {
-                            println!("Strategy failed: {}", msg);
+                            println!("Strategy failed: {:?}, {}", strategy, msg);
                         }
                         None // Continue searching
                     }
-                },
-            )
+                }
+            })
             .unwrap_or_else(|| {
                 Err(format!(
                     "No strategy worked out of {} tried",
@@ -221,18 +232,24 @@ impl Cartesian<'_> {
         start: &Joints,
         strategy: &Joints,
         poses: &Vec<AnnotatedPose>,
+        stop: &Arc<AtomicBool>,
     ) -> Result<Vec<AnnotatedJoints>, String> {
-        let onboarding = self.rrt.plan_rrt(start, strategy, self.robot)?;
-        let mut trace = Vec::with_capacity(onboarding.len() + poses.len() + 10);
+        let onboarding = self.rrt.plan_rrt(start, strategy, self.robot, stop)?;
+        if stop.load(Ordering::Relaxed) {
+            // Do not attempt Cartesian planning if we already cancelled.
+            return Err("Stopped".into());
+        }
 
+        let mut trace = Vec::with_capacity(onboarding.len() + poses.len() + 10);
+
         // Do not take the last value as it is same as 'strategy'
         for joints in onboarding.iter().take(onboarding.len() - 1) {
             trace.push(AnnotatedJoints {
                 joints: *joints,
                 flags: PathFlags::ONBOARDING,
             });
         }
-        
+
         // Push the strategy point, from here the move must be already CARTESIAN
         trace.push(AnnotatedJoints {
             joints: *strategy,
@@ -264,7 +281,7 @@ impl Cartesian<'_> {
                         } else {
                             PathFlags::TRACE | PathFlags::CARTESIAN | to.flags
                         };
-                    
+
                         trace.push(AnnotatedJoints {
                             joints: next,
                             flags: flags
@@ -291,7 +308,8 @@ impl Cartesian<'_> {
         if collides {
             return Err("Collision detected".into());
         }
-
+        // If all good, cancel other tasks still running
+        stop.store(true, Ordering::Relaxed);
         Ok(trace)
     }
 
@@ -335,7 +353,7 @@ impl Cartesian<'_> {
         if depth < self.linear_recursion_depth {
 
             // Try to bridge till them middle first, and then from the middle
-            // This will result in a shorter distance between from and to.            
+            // This will result in a shorter distance between from and to.
             let midpose = from.interpolate(to, DIV_RATIO);
             let middle_joints =
                 self.step_adaptive_linear_transition(starting, from, &midpose, depth + 1)?;

src/path_plan/rrt.rs

@@ -1,45 +1,50 @@
-use rrt::dual_rrt_connect;
-use std::time::Instant;
 use crate::kinematic_traits::{Joints, Kinematics};
 use crate::kinematics_with_shape::KinematicsWithShape;
+use crate::rrt_to::{dual_rrt_connect};
 use crate::utils::dump_joints;
+use std::sync::atomic::{AtomicBool};
+use std::time::Instant;
 
 #[derive(Debug)]
-/// Defines the RRT planner that relocates the robot between the two positions in a 
+/// Defines the RRT planner that relocates the robot between the two positions in a
 /// collision free way.
 pub struct RRTPlanner {
     /// Step size in the joint space (value in Radians). This should be small
-    /// enough to prevent robot colliding with something while moving 
-    /// in possibly less predictable way between the joints. 
+    /// enough to prevent robot colliding with something while moving
+    /// in possibly less predictable way between the joints.
     pub step_size_joint_space: f64,
-    
+
     /// The "max try" parameter of RRT algorithm, reasonable values
     /// are in order 1000 ... 4000
     pub max_try: usize,
-    
+
     /// Flag to print extra diagnostics if required.
-    pub debug: bool
+    pub debug: bool,
 }
 
 impl Default for RRTPlanner {
     fn default() -> Self {
         Self {
             step_size_joint_space: 3_f64.to_radians(),
             max_try: 2000,
-            debug: true, 
+            debug: true,
         }
     }
 }
 
 impl RRTPlanner {
-    /// Plans a path from `start` to `goal` joint configuration, 
+    /// Plans a path from `start` to `goal` joint configuration,
     /// using `KinematicsWithShape` for collision checking.
     /// start and goal are included into the returned path.
     fn plan_path(
         &self,
         kinematics: &KinematicsWithShape,
-        start: &Joints, goal: &Joints,
+        start: &Joints,
+        goal: &Joints,
+        stop: &AtomicBool,
     ) -> Result<Vec<Vec<f64>>, String> {
+        //return Ok(vec![Vec::from(start.clone()), Vec::from(goal.clone())]);
+
         let collision_free = |joint_angles: &[f64]| -> bool {
             let joints = &<Joints>::try_from(joint_angles).expect("Cannot convert vector to array");
             !kinematics.collides(joints)
@@ -48,16 +53,25 @@ impl RRTPlanner {
         // Constraint compliant random joint configuration generator.
         let random_joint_angles = || -> Vec<f64> {
             // RRT requires vector and we return array so convert
-            return kinematics.constraints()
-                .expect("Set joint ranges on kinematics").random_angles().to_vec();
+            return kinematics
+                .constraints()
+                .expect("Set joint ranges on kinematics")
+                .random_angles()
+                .to_vec();
         };
 
         // Plan the path with RRT
-        dual_rrt_connect(
-            start, goal, collision_free,
-            random_joint_angles, self.step_size_joint_space, // Step size in joint space
-            self.max_try,  // Max iterations
-        )
+        let path = dual_rrt_connect(
+            start,
+            goal,
+            collision_free,
+            random_joint_angles,
+            self.step_size_joint_space, // Step size in joint space
+            self.max_try,               // Max iterations
+            &stop,
+        );
+
+        path
     }
 
     fn convert_result(&self, data: Result<Vec<Vec<f64>>, String>) -> Result<Vec<Joints>, String> {
@@ -76,7 +90,6 @@ impl RRTPlanner {
         })
     }
 
-
     #[allow(dead_code)]
     fn print_summary(&self, planning_result: &Result<Vec<[f64; 6]>, String>) {
         match planning_result {
@@ -94,18 +107,31 @@ impl RRTPlanner {
 
     /// Plans collision - free relocation from 'start' into 'goal', using
     /// provided instance of KinematicsWithShape for both inverse kinematics and
-    /// collision avoidance. 
-    pub fn plan_rrt(&self, start: &Joints, goal: &Joints, kinematics: &KinematicsWithShape)
-                    -> Result<Vec<Joints>, String> {
+    /// collision avoidance.
+    pub fn plan_rrt(
+        &self,
+        start: &Joints,
+        goal: &Joints,
+        kinematics: &KinematicsWithShape,
+        stop: &AtomicBool,
+    ) -> Result<Vec<Joints>, String> {
+        println!("RRT started {:?} -> {:?}", start, goal);
         let started = Instant::now();
-        let path = self.plan_path(&kinematics, start, goal);
+        let path = self.plan_path(&kinematics, start, goal, stop);
         let spent = started.elapsed();
         let result = self.convert_result(path);
-        if self.debug {
-            // self.print_summary(&result);
-            println!("RRT Took {:?}", &spent);
+
+        match &result {
+            Ok(path) => {
+                println!("RRT steps: {}", &path.len());
+            }
+            Err(error_message) => {
+                println!("Direct RRT failed: {}", error_message);
+            }
         }
+        // self.print_summary(&result);
+        println!("RRT Took {:?} for {:?} -> {:?}", &spent, start, goal);
+
         result
     }
-}    
-
+}