First working version of the rust path planning

Author: Flova

src/bitbots_navigation/bitbots_path_planning/config/path_planning.yaml

@@ -5,7 +5,7 @@ bitbots_path_planning:
     rate: 20.0
 
     map:
-      planning_frame: map
+      frame: map
       robot_update_topic: robots_relative_filtered
       ball_diameter: 0.13
       inflation:

src/bitbots_navigation/bitbots_path_planning/src/controller.rs

@@ -37,8 +37,7 @@ pub enum ControllerStepError {
     LocalizationError(#[from] TfError),
 }
 
-pub struct Controller<'a> {
-    tf: &'a TfListener,
+pub struct Controller {
     params: Arc<StdMutex<Params>>,
 }
 
@@ -53,9 +52,9 @@ fn get_yaw_from_quaternion(quaternion: &Quaternion) -> f64 {
     .0 as f64
 }
 
-impl<'a> Controller<'a> {
-    pub fn new(tf: &'a TfListener, params: Arc<StdMutex<Params>>) -> Self {
-        Self { tf, params }
+impl Controller {
+    pub fn new(params: Arc<StdMutex<Params>>) -> Self {
+        Self { params }
     }
 
     // Calculates a command velocity based on a given path
@@ -64,15 +63,12 @@ impl<'a> Controller<'a> {
 
         let params = self.params.lock().unwrap().clone();
 
-        let my_position = self
-            .tf
-            .lookup_transform(
-                &params.map.frame,
-                &params.base_footprint_frame,
-                Time::default(),
-            )
-            .map_err(|e| ControllerStepError::LocalizationError(e))?
-            .transform;
+        let my_position = path
+            .poses
+            .first()
+            .expect("Path must contain at least one pose")
+            .pose
+            .clone();
 
         let end_pose = &path
             .poses
@@ -88,59 +84,43 @@ impl<'a> Controller<'a> {
             }
         };
 
-        let walk_angle = (goal_pose.position.y - my_position.translation.y)
-            .atan2(goal_pose.position.x - my_position.translation.x);
+        let walk_angle = (goal_pose.position.y - my_position.position.y)
+            .atan2(goal_pose.position.x - my_position.position.x);
 
         let distance = if path.poses.len() < 3 {
-            (end_pose.position.x - my_position.translation.x)
-                .hypot(end_pose.position.y - my_position.translation.y)
+            (end_pose.position.x - my_position.position.x)
+                .hypot(end_pose.position.y - my_position.position.y)
         } else {
             path.poses
                 .iter()
                 .fold(
-                    (0.0, my_position.translation),
+                    (0.0, my_position.position),
                     |(distance, last_pose), pose| {
                         let dx = pose.pose.position.x - last_pose.x;
                         let dy = pose.pose.position.y - last_pose.y;
-                        (
-                            distance + dx.hypot(dy),
-                            Vector3 {
-                                x: pose.pose.position.x,
-                                y: pose.pose.position.y,
-                                z: 0.0,
-                            },
-                        )
+                        (distance + dx.hypot(dy), pose.pose.position.clone())
                     },
                 )
                 .0
         };
 
-        let walk_vel = distance
-            * params.controller.translation_slow_down_factor;
+        let walk_vel = distance * params.controller.translation_slow_down_factor;
 
-        let diff = if distance
-            > params
-                .controller
-                .orient_to_goal_distance
-        {
-            walk_angle - get_yaw_from_quaternion(&my_position.rotation)
+        let diff = if distance > params.controller.orient_to_goal_distance {
+            walk_angle - get_yaw_from_quaternion(&my_position.orientation)
         } else {
             get_yaw_from_quaternion(&end_pose.orientation)
-                - get_yaw_from_quaternion(&my_position.rotation)
+                - get_yaw_from_quaternion(&my_position.orientation)
         };
 
-        let min_angle = diff % std::f64::consts::TAU; // TODO check math / remainder implementation
+        let min_angle = diff.sin().atan2(diff.cos());
 
-        cmd_vel.angular.z = (min_angle
-            * params
-                .controller
-                .rotation_slow_down_factor)
-            .clamp(
-                -params.controller.max_rotation_vel,
-                params.controller.max_rotation_vel,
-            );
+        cmd_vel.angular.z = (min_angle * params.controller.rotation_slow_down_factor).clamp(
+            -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 = walk_angle - get_yaw_from_quaternion(&my_position.orientation);
         let local_heading_vector_x = local_heading.cos();
         let local_heading_vector_y = local_heading.sin();
 

src/bitbots_navigation/bitbots_path_planning/src/main.rs

@@ -60,6 +60,12 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
     let mut avoid_ball_sub = node
         .subscribe::<Bool>("ball_obstacle_active", r2r::QosProfile::default())?
         .fuse();
+    let mut pose_sub = node
+        .subscribe::<r2r::geometry_msgs::msg::PoseWithCovarianceStamped>(
+            "/pose_with_covariance",
+            r2r::QosProfile::default(),
+        )?
+        .fuse();
 
     let cmd_vel_pub = node.create_publisher::<Twist>("cmd_vel", r2r::QosProfile::default())?;
     let path_pub = node.create_publisher::<Path>("path", r2r::QosProfile::default())?;
@@ -71,45 +77,46 @@ async fn main() -> Result<(), Box<dyn std::error::Error>> {
         1. / params.lock().unwrap().rate,
     ))?;
 
-    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, params.clone());
-        let controller = Controller::new(&tf_listener, params.clone());
+        let mut planner = Planner::new(params.clone());
+        let controller = Controller::new(params.clone());
 
         // Handle events
         loop {
             tokio::select! {
+                Some(msg) = pose_sub.next() => {
+                    planner.update_own_pose(msg);
+                },
                 _ = 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() {
+                        let path = match planner.step() {
                             Err(e) => {
                                 r2r::log_warn!(&nl, "Planner step failed: {:}", e);
+                                continue;
                             },
                             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) {
-                                    Err(e) => {
-                                        r2r::log_warn!(&nl, "Controller step failed: {:}", e);
-                                    },
-                                    Ok((cmd_vel, carrot_point)) => {
-                                        // Publish the walk command to control the robot
-                                        cmd_vel_pub.publish(&cmd_vel).expect("Publishing command velocity failed");
-                                        // Publish the carrot point for visualization
-                                        carrot_pub.publish(&carrot_point).expect("Publishing carrot failed");
-                                    }
-                                }
+                                path
+                            }
+                        };
+                        // Calculate the command velocity to follow the given path
+                        match controller.step(&path) {
+                            Err(e) => {
+                                r2r::log_warn!(&nl, "Controller step failed: {:}", e);
+                                continue;
+                            },
+                            Ok((cmd_vel, carrot_point)) => {
+                                // Publish the walk command to control the robot
+                                cmd_vel_pub.publish(&cmd_vel).expect("Publishing command velocity failed");
+                                // Publish the carrot point for visualization
+                                carrot_pub.publish(&carrot_point).expect("Publishing carrot failed");
                             }
                         }
                     }

src/bitbots_navigation/bitbots_path_planning/src/planner.rs

@@ -7,6 +7,7 @@ use bitbots_rust_nav::map::ObstacleMapConfig;
 use bitbots_rust_nav::obstacle::RoundObstacle;
 use r2r::builtin_interfaces::msg::Time;
 use r2r::geometry_msgs::msg::PoseStamped;
+use r2r::geometry_msgs::msg::PoseWithCovarianceStamped;
 use r2r::nav_msgs::msg::Path;
 use r2r::std_msgs::msg::Header;
 use r2r::RosParams;
@@ -32,25 +33,25 @@ pub struct InflationParams {
     pub obstacle_margin: f64,
 }
 
-pub struct Planner<'a> {
-    tf: &'a TfListener,
+pub struct Planner {
+    current_pose: Option<PoseStamped>,
     goal: Option<PoseStamped>,
     ball_obstacle_active: bool,
     params: Arc<StdMutex<Params>>,
 }
 
 #[derive(Error, Debug)]
 pub enum PlannerStepError {
-    #[error("Could not determine the robot location. Tf error: {0}")]
-    LocalizationError(#[from] TfError),
+    #[error("Could not determine the robot location.")]
+    LocalizationError(),
     #[error("No goal was given when step was called")]
     NoGoalError(),
 }
 
-impl<'a> Planner<'a> {
-    pub fn new(tf: &'a TfListener, params: Arc<StdMutex<Params>>) -> Self {
+impl Planner {
+    pub fn new(params: Arc<StdMutex<Params>>) -> Self {
         Self {
-            tf,
+            current_pose: None,
             goal: None,
             ball_obstacle_active: false,
             params,
@@ -79,50 +80,55 @@ impl<'a> Planner<'a> {
         self.ball_obstacle_active = state;
     }
 
+    // Updates the current robot pose
+    pub fn update_own_pose(&mut self, pose: PoseWithCovarianceStamped) {
+        self.current_pose = Some(PoseStamped {
+            header: pose.header,
+            pose: pose.pose.pose,
+        });
+    }
+
     // Compute the next path to the goal
     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
-            .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 my_pose = self
+            .current_pose
+            .as_ref()
+            .ok_or(PlannerStepError::LocalizationError())?
+            .clone(); // Todo handle timeout
 
         let map_config = ObstacleMapConfig::new(
             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
 
         // TODO add ball to map
 
-        let path = ObstacleMap::new(map_config, obstacles)
+        let path: Vec<_> = ObstacleMap::new(map_config, obstacles)
             .shortest_path(
-                (my_position.x, my_position.y),
-                (goal.pose.position.x, goal.pose.position.x),
+                (my_pose.pose.position.x, my_pose.pose.position.y),
+                (goal.pose.position.x, goal.pose.position.y),
             )
             .iter()
             .map(|position| {
                 let mut pose = PoseStamped::default();
                 pose.pose.position.x = position.0;
                 pose.pose.position.y = position.1;
-                pose
+                pose.clone()
             })
+            .collect();
+
+        let path: Vec<PoseStamped> = std::iter::once(my_pose)
+            .chain(path)
             .chain([goal.clone()])
             .collect();