Current state of path planning migration

Author: Flova

src/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml

@@ -0,0 +1,25 @@
+bitbots_path_planning:
+  ros__parameters:
+    base_footprint_frame: base_footprint
+    tf_buffer_duration: 5.0
+    rate: 20.0
+
+    map:
+      planning_frame: map
+      robot_update_topic: robots_relative_filtered
+      ball_diameter: 0.13
+      inflation:
+        robot_radius: 0.3
+        obstacle_margin: 0.1
+
+    controller:
+      carrot_distance: 1
+      max_rotation_vel: 0.4
+      max_vel_x: 0.12
+      min_vel_x: -0.06
+      max_vel_y: 0.07
+      smoothing_tau: 0.1
+      rotation_i_factor: 0.0
+      rotation_slow_down_factor: 0.6
+      translation_slow_down_factor: 0.6
+      orient_to_goal_distance: 0.5

src/bitbots_navigation/bitbots_path_planning/launch/path_planning.launch

@@ -1,7 +1,8 @@
 <launch>
     <arg name="sim" default="false" description="true: activates simulation time" />
 
-    <node pkg="bitbots_path_planning" exec="path_planning" name="bitbots_path_planning" output="screen">
+    <node pkg="bitbots_path_planning" exec="bitbots_path_planning" name="bitbots_path_planning" output="screen">
         <param name="use_sim_time" value="$(var sim)"/>
+        <param from="$(find-pkg-share bitbots_path_planning)/config/path_planning.yaml"/>
     </node>
 </launch>

src/bitbots_navigation/bitbots_path_planning/src/controller.rs

@@ -11,10 +11,10 @@ use crate::Params;
 
 use thiserror::Error;
 
-#[derive(RosParams, Default, Debug)]
+#[derive(RosParams, Default, Debug, Clone)]
 pub struct ControllerParams {
     // The distance to the carrot that we want to reach on the path
-    carrot_distance: f32,
+    carrot_distance: i32,
     // The maximum rotation velocity of the robot in rad/s around the z-axis
     max_rotation_vel: f64,
     // Clamped p gain of the rotation controller
@@ -23,6 +23,12 @@ pub struct ControllerParams {
     translation_slow_down_factor: f64,
     // Distance at which we switch from orienting towards the path to orienting towards the goal poses orientation (in meters)
     orient_to_goal_distance: f64,
+    // Maximum velocity we want to reach in different directions (base_footprint coordinate system)
+    max_vel_x: f64,
+    // Minimum velocity we want to reach in different directions (base_footprint coordinate system)
+    min_vel_x: f64,
+    // Maximum velocity we want to reach in different directions (base_footprint coordinate system)
+    max_vel_y: f64,
 }
 
 #[derive(Error, Debug)]
@@ -31,8 +37,8 @@ pub enum ControllerStepError {
     LocalizationError(#[from] TfError),
 }
 
-pub struct Controller {
-    tf: Arc<Mutex<TfListener>>,
+pub struct Controller<'a> {
+    tf: &'a TfListener,
     params: Arc<StdMutex<Params>>,
 }
 
@@ -47,22 +53,22 @@ fn get_yaw_from_quaternion(quaternion: &Quaternion) -> f64 {
     .0 as f64
 }
 
-impl Controller {
-    pub fn new(tf: Arc<Mutex<TfListener>>, params: Arc<StdMutex<Params>>) -> Self {
+impl<'a> Controller<'a> {
+    pub fn new(tf: &'a TfListener, params: Arc<StdMutex<Params>>) -> Self {
         Self { tf, params }
     }
 
     // Calculates a command velocity based on a given path
-    pub async fn step(&self, path: &Path) -> Result<(Twist, PointStamped), ControllerStepError> {
+    pub fn step(&self, path: &Path) -> Result<(Twist, PointStamped), ControllerStepError> {
         let mut cmd_vel = Twist::default();
 
+        let params = self.params.lock().unwrap().clone();
+
         let my_position = self
             .tf
-            .lock()
-            .await
             .lookup_transform(
-                &self.params.lock().unwrap().map.frame,
-                &self.params.lock().unwrap().base_footprint_frame,
+                &params.map.frame,
+                &params.base_footprint_frame,
                 Time::default(),
             )
             .map_err(|e| ControllerStepError::LocalizationError(e))?
@@ -74,7 +80,7 @@ impl Controller {
             .expect("Expect path to contain at least one pose")
             .pose;
         let goal_pose = {
-            let carrot_distance = self.params.lock().unwrap().controller.carrot_distance as usize;
+            let carrot_distance = params.controller.carrot_distance as usize;
             if path.poses.len() > carrot_distance {
                 &path.poses[path.poses.len() - carrot_distance].pose
             } else {
@@ -110,18 +116,10 @@ impl Controller {
         };
 
         let walk_vel = distance
-            * self
-                .params
-                .lock()
-                .unwrap()
-                .controller
-                .translation_slow_down_factor;
+            * params.controller.translation_slow_down_factor;
 
         let diff = if distance
-            > self
-                .params
-                .lock()
-                .unwrap()
+            > params
                 .controller
                 .orient_to_goal_distance
         {
@@ -134,21 +132,41 @@ impl Controller {
         let min_angle = diff % std::f64::consts::TAU; // TODO check math / remainder implementation
 
         cmd_vel.angular.z = (min_angle
-            * self
-                .params
-                .lock()
-                .unwrap()
+            * params
                 .controller
                 .rotation_slow_down_factor)
             .clamp(
-                -self.params.lock().unwrap().controller.max_rotation_vel,
-                self.params.lock().unwrap().controller.max_rotation_vel,
+                -params.controller.max_rotation_vel,
+                params.controller.max_rotation_vel,
             );
 
         let local_heading = walk_angle - get_yaw_from_quaternion(&my_position.rotation);
         let local_heading_vector_x = local_heading.cos();
         let local_heading_vector_y = local_heading.sin();
 
+        // Returns interpolates the x and y maximum velocity linearly based on our heading
+        // See https://github.com/bit-bots/bitbots_main/issues/366#issuecomment-2161460917
+        fn interpolate_max_vel(x_limit: f64, y_limit: f64, target_x: f64, target_y: f64) -> f64 {
+            let n = (x_limit * target_y.abs()) + (y_limit * target_x);
+            let intersection_x = (x_limit * y_limit * target_x) / n;
+            let intersection_y = (x_limit * y_limit * target_y.abs()) / n;
+            intersection_x.hypot(intersection_y)
+        }
+
+        let walk_vel = walk_vel.min(interpolate_max_vel(
+            if local_heading_vector_x > 0. {
+                params.controller.max_vel_x
+            } else {
+                params.controller.min_vel_x
+            },
+            params.controller.max_vel_y,
+            local_heading_vector_x,
+            local_heading_vector_y,
+        ));
+
+        cmd_vel.linear.x = walk_vel * local_heading_vector_x;
+        cmd_vel.linear.y = walk_vel * local_heading_vector_y;
+
         let carrot_point = PointStamped {
             header: path.header.clone(),
             point: goal_pose.position.clone(),

src/bitbots_navigation/bitbots_path_planning/src/main.rs

@@ -14,7 +14,7 @@ use r2r::RosParams;
 mod controller;
 mod planner;
 
-#[derive(RosParams, Default, Debug)]
+#[derive(RosParams, Default, Debug, Clone)]
 pub struct Params {
     // The rate at which the path planning is executed
     pub rate: f32,
@@ -71,32 +71,35 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
         1. / params.lock().unwrap().rate,
     ))?;
 
-    let tf_listener = Arc::new(Mutex::new(tf_r2r::TfListener::new(&mut node)));
+    let tf_listener = tf_r2r::TfListener::new(&mut node);
 
     // Share the node between tasks / threads
     let node = Arc::new(Mutex::new(node));
     let spin_node = node.clone();
 
     tokio::task::spawn(async move {
         // Define a thread-local state
-        let mut planner = Planner::new(tf_listener.clone(), params.clone());
-        let controller = Controller::new(tf_listener.clone(), params.clone());
+        let mut planner = Planner::new(&tf_listener, params.clone());
+        let controller = Controller::new(&tf_listener, params.clone());
 
         // Handle events
         loop {
             tokio::select! {
                 _ = timer.tick() => {
+                    r2r::log_warn!(&nl, "Planner step");
                     if planner.active() {
+                        r2r::log_warn!(&nl, "Planner active");
                         // Calculate the path to the goal pose considering the current map
-                        match planner.step().await {
+                        match planner.step() {
                             Err(e) => {
                                 r2r::log_warn!(&nl, "Planner step failed: {:}", e);
                             },
                             Ok(path) => {
+                                r2r::log_warn!(&nl, "Got path");
                                 // Publish the path for visualization
                                 path_pub.publish(&path).expect("Publishing the path failed");
                                 // Calculate the command velocity to follow the given path
-                                match controller.step(&path).await {
+                                match controller.step(&path) {
                                     Err(e) => {
                                         r2r::log_warn!(&nl, "Controller step failed: {:}", e);
                                     },
@@ -112,6 +115,7 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
                     }
                 },
                 Some(msg) = goal_sub.next() => {
+                    r2r::log_warn!(&nl, "Got goal");
                     planner.set_goal(msg);
                 },
                 _ = cancel_sub.next() => {
@@ -124,11 +128,19 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
         }
     });
 
-    // Spin the underlying rcl node object
-    loop {
-        spin_node
-            .lock()
-            .await
-            .spin_once(std::time::Duration::from_millis(100));
-    }
+    // Spin the node
+    tokio::task::spawn(async move {
+        loop {
+            spin_node
+                .lock()
+                .await
+                .spin_once(std::time::Duration::from_millis(100));
+            tokio::task::yield_now().await; // Let the other tasks run too
+        }
+    });
+
+    // Await shutdown signal
+    tokio::signal::ctrl_c().await?;
+
+    Ok(())
 }

src/bitbots_navigation/bitbots_path_planning/src/planner.rs

@@ -14,7 +14,7 @@ use tf_r2r::{TfError, TfListener};
 use thiserror::Error;
 use tokio::sync::Mutex;
 
-#[derive(RosParams, Default, Debug)]
+#[derive(RosParams, Default, Debug, Clone)]
 pub struct MapParams {
     // The frame in which the path planning is done
     pub frame: String,
@@ -24,31 +24,31 @@ pub struct MapParams {
     pub inflation: InflationParams,
 }
 
-#[derive(RosParams, Default, Debug)]
+#[derive(RosParams, Default, Debug, Clone)]
 pub struct InflationParams {
     // Radius of a circle on the ground that represents the space occupied by our robot. Instead of planning with both a robot polygon/circle and an obstacle polygon, we just inflate the obstacles and assume the robot is a point. This is faster and simpler
     pub robot_radius: f64,
     // Distance we want to keep to obstacles when planning a path around them. No immediate action is required if the robot is closer than this distance to an obstacle, but we don't consider paths this close during the visibility graph generation.
     pub obstacle_margin: f64,
 }
 
-pub struct Planner {
-    tf: Arc<Mutex<TfListener>>,
+pub struct Planner<'a> {
+    tf: &'a TfListener,
     goal: Option<PoseStamped>,
     ball_obstacle_active: bool,
     params: Arc<StdMutex<Params>>,
 }
 
 #[derive(Error, Debug)]
 pub enum PlannerStepError {
-    #[error("Could not determine the robot location")]
+    #[error("Could not determine the robot location. Tf error: {0}")]
     LocalizationError(#[from] TfError),
     #[error("No goal was given when step was called")]
     NoGoalError(),
 }
 
-impl Planner {
-    pub fn new(tf: Arc<Mutex<TfListener>>, params: Arc<StdMutex<Params>>) -> Self {
+impl<'a> Planner<'a> {
+    pub fn new(tf: &'a TfListener, params: Arc<StdMutex<Params>>) -> Self {
         Self {
             tf,
             goal: None,
@@ -80,30 +80,32 @@ impl Planner {
     }
 
     // Compute the next path to the goal
-    pub async fn step(&self) -> Result<Path, PlannerStepError> {
+    pub fn step(&self) -> Result<Path, PlannerStepError> {
         // Check if we have a goal
         let goal = self.goal.as_ref().ok_or(PlannerStepError::NoGoalError())?;
 
+        let params = self.params.lock().unwrap().clone();
+
+        r2r::log_warn!("bitbots_path_planning", "Fetched goal {:?}", params.map.inflation.robot_radius);
+
         // Get our current position
         let my_position = self
             .tf
-            .lock()
-            .await
-            .lookup_transform(
-                &self.params.lock().unwrap().map.frame,
-                &self.params.lock().unwrap().base_footprint_frame,
-                Time::default(),
-            )
+            .lookup_transform(&params.map.frame, &params.base_footprint_frame, Time::default())
             .map_err(|e| PlannerStepError::LocalizationError(e))?
             .transform
             .translation;
 
+        r2r::log_warn!("bitbots_path_planning", "Fetched our position");
+
         let map_config = ObstacleMapConfig::new(
-            self.params.lock().unwrap().map.inflation.robot_radius,
-            self.params.lock().unwrap().map.inflation.obstacle_margin,
+            params.map.inflation.robot_radius,
+            params.map.inflation.obstacle_margin,
             12,
         );
 
+        r2r::log_warn!("bitbots_path_planning", "Made obstacle map config");
+
         let obstacles = vec![];
         // TODO add robot to map