Fix random walk continual obstacle avoidance

docs/robot/autonomy/3_local/planning/index.md

@@ -4,4 +4,4 @@ Part of the local planner is the Waypoint Manager.
 
 The Waypoint Manager subscribes to the global waypoints and the drone's current position and publishes the next waypoint to the local planner.
 
-We plan for this baseline to be DROAN
+We plan for this baseline to be DROAN.

mkdocs.yml

@@ -69,8 +69,16 @@ nav:
               - docs/robot/autonomy/3_local/index.md
               - World Model:
                 - docs/robot/autonomy/3_local/world_model/index.md
+                - DROAN (Obstacle Avoidance World Model):
+                  - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_expansion/README.md
+                  - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/README.md
+                  - robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/README.md
               - Planning:
                 - docs/robot/autonomy/3_local/planning/index.md
+                - robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/README.md
+                - robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md
+                - DROAN (Obstacle Avoidance Planner):
+                  - robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/README.md
               - Controls:
                 - docs/robot/autonomy/3_local/controls/index.md
                 - robot/ros_ws/src/autonomy/3_local/c_controls/trajectory_controller/README.md

robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/README.md

@@ -0,0 +1,6 @@
+# Disparity Graph
+
+
+Contact: Andrew Jong
+
+Docs TODO. Help appreciated.

robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/README.md

@@ -0,0 +1,6 @@
+# Disparity Graph Cost Map
+
+
+Contact: Andrew Jong
+
+Docs TODO. Help appreciated.

robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/README.md

@@ -0,0 +1,18 @@
+# DROAN Local Planner
+
+
+This is part of a set of packages that performs local obstacle avoidance from stereo depth. The method is based on the publication ["DROAN - Disparity-space representation for obstacle avoidance."](https://www.ri.cmu.edu/app/uploads/2018/01/root.pdf). Essentially, the local world model makes a C-space expansion around detected obstacles in the disparity image. The local planner then plans a path by scoring the best trajectory from a trajectory library in this expanded space.
+
+The packages are
+
+ 1. [disparity_expansion](../../a_world_models/disparity_expansion/README.md)   (takes in stereo disparity and expands it to a 3D point cloud)
+ 2. [disparity_graph](../../a_world_models/disparity_graph/README.md)        (creates a graph where each node is a drone pose and 3D point cloud observation)
+ 3. [disparity_graph_cost_map](../../a_world_models/disparity_graph_cost_map/README.md)  (creates a cost map from the disparity graph)
+ 4. droan_local_planner  (this package. uses the cost map to plan a path)
+
+
+DROAN local planner takes the global plan, and finds the global plan's closest point to the drone.
+DROAN trims the global plan to be from that point to the end of the global plan.
+
+Consequently, DROAN currently does NOT use a waypoint manager, and is NOT guaranteed to reach every waypoint on the global plan, especially if the global plan loops back on itself.
+Ideally in the future we will use a proper waypoint manager.

robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml

@@ -10,17 +10,17 @@
     dt: 0.2
     ht: 2.4
     ht_long: 3.0
-    max_velocity: 20.0
+    max_velocity: 1.0
     magnitude: 0.5
 
     # DROAN parameters
     robot_radius: 0.325  # spirit drone blade to blade
-    safety_cost_weight: 1.0
-    forward_progress_forgiveness_weight: 0.5
+    safety_cost_weight: 5.0
+    forward_progress_forgiveness_weight: 0.1
 
     # map parameters. see disparity_graph_cost_map.cpp
     cost_map: disparity_graph_cost_map::DisparityGraphCostMap
-    obstacle_check_radius: 1.0 # this value MUST be strictly greater than the robot radius
+    obstacle_check_radius: 2.0 # this value MUST be strictly greater than the robot radius
     obstacle_check_num_points: 5
     # disparity graph parameters. see disparity_graph.hpp
     fixed_frame: "map"
@@ -40,4 +40,7 @@
 
     # CUSTOM_WAYPOINT mode parameters
     custom_waypoint_timeout_factor: 0.3
-    custom_waypoint_distance_threshold: 0.5
+    custom_waypoint_distance_threshold: 0.5
+
+    # Disparity Graph parameters
+    graph_size: 5

robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp

@@ -192,12 +192,12 @@ class DroanLocalPlanner : public rclcpp::Node {
         double trajectory_distance;
         bool is_valid = global_plan.get_trajectory_distance_at_closest_point(look_ahead_position,
                                                                              &trajectory_distance);
-        // trim the global plan to only the next 10 meters
+        // trim the global plan to only the next 100 meters
         if (is_valid) {
-            this->global_plan_trajectory_distance += trajectory_distance;
-            global_plan = global_plan.trim_trajectory_between_distances(
-                this->global_plan_trajectory_distance,
-                this->global_plan_trajectory_distance + 10.0);
+            trajectory_distance = std::max(trajectory_distance, this->global_plan_trajectory_distance);
+            global_plan = global_plan.trim_trajectory_between_distances(trajectory_distance, trajectory_distance + std::numeric_limits<double>::infinity());
+            RCLCPP_INFO_STREAM(this->get_logger(), "using global plan from distance " << trajectory_distance);
+            this->global_plan_trajectory_distance = trajectory_distance;
         } else {
             RCLCPP_INFO(this->get_logger(), "invalid");
         }

robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md

@@ -1,4 +1,6 @@
-### TakeoffLandingPlanner
+# Takeoff Landing Planner
 
 
-Author:
+Author: John Keller
+
+DOCS TODO. Help appreciated.

robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml

@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
-    takeoff_height: 1.0
-    high_takeoff_height: 2.0
+    takeoff_height: 2.0
+    high_takeoff_height: 10.0
     takeoff_landing_velocity: 0.3
     takeoff_acceptance_distance: 0.3
     takeoff_acceptance_time: 10.0

robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/README.md

@@ -0,0 +1,7 @@
+# Trajectory Library
+
+Contact: John Keller
+
+Defines some basic trajectory classes and functions for generating and manipulating trajectories.
+
+Docs TODO. Help appreciated.

robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml

@@ -57,13 +57,13 @@
         output="screen">
         <param name="tf_prefix" value="" />
         <param name="target_frame" value="map" />
-        <param name="tracking_point_distance_limit" value="10.5" />
+        <param name="tracking_point_distance_limit" value="1000.5" />
         <param name="velocity_look_ahead_time" value="0.9" />
         <!-- look ahead time controls the speed, greater is faster -->
         <param name="look_ahead_time" value="1.0" />
         <param name="virtual_tracking_ahead_time" value="0.5" />
         <param name="min_virtual_tracking_velocity" value="0.1" />
-        <param name="sphere_radius" value="1." />
+        <param name="sphere_radius" value="1.0" />
         <param name="ff_min_velocity" value="0." />
         <param name="search_ahead_factor" value="1.5" />
         <param name="transition_velocity_scale" value="1.0" />

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md

@@ -2,6 +2,9 @@
 
 The Random Walk Planner serves as a baseline global planner for stress testing system autonomy. Unlike more informed and intelligent planners, the Random Walk Planner generates a series of random trajectories to evaluate system robustness. Using the map published by VDB, the planner will generate and publish multiple linked straight-line trajectories, checking for collisions along these paths.
 
+![random walk image](./random_walk.png)
+The blue line is the global plan generated by the random walk. The yellow line shows the past trajectory followed by the local planner when pursuing the previous random global plans.
+
 ## Functionality
 
 Upon activation by the behavior tree, the Random Walk Planner will:
@@ -21,7 +24,7 @@ This loop continues, allowing the system to explore various trajectories and str
 | `max_z_change_m`           | Maximum allowed change in height (z-axis) between the start and goal points.|
 | `collision_padding_m`      | Extra padding (in meters) added around a voxel's dimensions when checking for collisions.|
 | `path_end_threshold_m`     | Distance threshold (in meters) for considering the current path completed and generating a new one.|
-| `max_z_angle_change_rad`   | Maximum allowed change in angle (in radians) between consecutive straight-line segments to ensure a relatively consistent direction.|
+| `max_yaw_change_degrees`   | Maximum allowed change in angle (in radians) between consecutive straight-line segments to ensure a relatively consistent direction.|
 | `robot_frame_id`           | The frame name for the robot's base frame to look up the transform from the robot position to the world.|
 
 ## Services

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml

@@ -12,10 +12,13 @@
     num_paths_to_generate: 5 # how many random walk paths to string together
 
     # Random Walk Planner Parameters
-    max_start_to_goal_dist_m: 5.0 # how max distance the goal can be from the current robot position
+    max_start_to_goal_dist_m: 10.0 # how max distance the goal can be from the current robot position
     checking_point_cnt: 100 # how many points to collision check between the start and goal points
-    max_z_change_m: 0.1 # max height that the goal can deviate from the current height
-    collision_padding_m: 0.1 # how much space should the path have to any obstacles
-    path_end_threshold_m: 0.1 # how close to the goal will the path be considered complete
-    max_z_angle_change_rad: 1.0 # how much the goal can deviate from the current orientation
+    max_z_change_m: 2.0 # max height that the goal can deviate from the current height
+    collision_padding_m: 1.0 # how much space should the path have to any obstacles
+    path_end_threshold_m: 3.0 # how close to the goal will the path be considered complete
+    max_yaw_change_degrees: 180.0 # how much the goal can deviate from the current orientation
 
+    # Stall detection parameters
+    position_change_threshold: 0.1  # Minimum distance (meters) to consider as movement
+    stall_timeout_seconds: 3.0     # Time without movement before clearing plan

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp

@@ -19,7 +19,7 @@ struct init_params {
     float max_z_change_m;
     float collision_padding_m;
     float path_end_threshold_m;
-    float max_z_angle_change_rad;
+    float max_yaw_change_degrees;
     std::tuple<float, float, float> voxel_size_m;
 };
 
@@ -43,7 +43,7 @@ class RandomWalkPlanner {
     int checking_point_cnt;
     float max_z_change_m_;
     float collision_padding_m;
-    float max_z_angle_change_rad;
+    float max_yaw_change_degrees;
 
     // Variables
     std::tuple<float, float, float> voxel_size_m;

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp

@@ -52,6 +52,10 @@ class RandomWalkNode : public rclcpp::Node {
 
     geometry_msgs::msg::Transform current_location;       // x, y, z, yaw
     geometry_msgs::msg::Transform current_goal_location;  // x, y, z, yaw
+    geometry_msgs::msg::Transform last_location;         // Last recorded position
+    rclcpp::Time last_position_change;                  // Time of last position change
+    double position_change_threshold = 0.1;       // Minimum distance (meters) to consider as movement
+    double stall_timeout_seconds = 5.0;          // Time without movement before clearing plan
 
     // Callbacks
     void mapCallback(const visualization_msgs::msg::Marker::SharedPtr msg);

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp

@@ -11,7 +11,7 @@ RandomWalkPlanner::RandomWalkPlanner(init_params params)
     this->voxel_size_m = params.voxel_size_m;
     this->collision_padding_m = params.collision_padding_m;
     this->path_end_threshold_m = params.path_end_threshold_m;
-    this->max_z_angle_change_rad = params.max_z_angle_change_rad;
+    this->max_yaw_change_degrees = params.max_yaw_change_degrees;
 }
 
 std::optional<Path> RandomWalkPlanner::generate_straight_rand_path(
@@ -110,6 +110,14 @@ bool RandomWalkPlanner::check_if_collided_single_voxel(
 
 bool RandomWalkPlanner::check_if_collided(const std::tuple<float, float, float>& point)
 {
+    // make sure the point is within the bounds -30 to 30
+    if (std::get<0>(point) > 30 || std::get<0>(point) < -30 || std::get<1>(point) > 30 ||
+        std::get<1>(point) < -30 || std::get<2>(point) > 30 || std::get<2>(point) < -30)
+    {
+        return true;
+    }
+
+
     std::lock_guard<std::mutex> lock(this->mutex);
     for (const std::tuple<float, float, float>& voxel : this->voxel_points)
     {
@@ -144,6 +152,7 @@ std::tuple<float, float, float> RandomWalkPlanner::generate_goal_point(
         float rand_x = std::get<0>(start_point) + delta_x;
         float rand_y = std::get<1>(start_point) + delta_y;
         float rand_z = std::get<2>(start_point) + delta_z;
+        rand_z = std::max(0.5f, rand_z); // ensure don't go below the ground
         std::tuple<float, float, float> rand_point(rand_x, rand_y, rand_z);
         std::tuple<float, float, float> start_point_wo_yaw(
             std::get<0>(start_point), std::get<1>(start_point), std::get<2>(start_point));
@@ -152,7 +161,7 @@ std::tuple<float, float, float> RandomWalkPlanner::generate_goal_point(
                                      std::get<0>(rand_point) - std::get<0>(start_point_wo_yaw));
         float angle_diff = std::abs(std::get<3>(start_point) - new_angle);
         // if the z value of the point is low enough
-        if (angle_diff < this->max_z_angle_change_rad)
+        if (angle_diff <= this->max_yaw_change_degrees * 3.14159265359 / 180)
         {
             // if the point is close enough to the start point
             if (dist < this->max_start_to_goal_dist_m_)

robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp

@@ -1,4 +1,3 @@
-
 #include "../include/random_walk_node.hpp"
 
 #include "../include/random_walk_logic.hpp"
@@ -76,9 +75,20 @@ std::optional<init_params> RandomWalkNode::readParameters() {
         RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: path_end_threshold_m");
         return std::optional<init_params>{};
     }
-    this->declare_parameter<float>("max_z_angle_change_rad");
-    if (!this->get_parameter("max_z_angle_change_rad", params.max_z_angle_change_rad)) {
-        RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_z_angle_change_rad");
+    this->declare_parameter<float>("max_yaw_change_degrees");
+    if (!this->get_parameter("max_yaw_change_degrees", params.max_yaw_change_degrees)) {
+        RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: max_yaw_change_degrees");
+        return std::optional<init_params>{};
+    }
+    this->declare_parameter<float>("position_change_threshold");
+    if (!this->get_parameter("position_change_threshold", this->position_change_threshold)) {
+
+        RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: position_change_threshold");
+        return std::optional<init_params>{};
+    }
+    this->declare_parameter<float>("stall_timeout_seconds");
+    if (!this->get_parameter("stall_timeout_seconds", this->stall_timeout_seconds)) {
+        RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: stall_timeout_seconds");
         return std::optional<init_params>{};
     }
     return params;
@@ -257,6 +267,7 @@ void RandomWalkNode::publish_plan() {
     this->pub_global_plan->publish(full_path);
     RCLCPP_INFO(this->get_logger(), "Published full path");
 }
+
 void RandomWalkNode::timerCallback() {
     // get current TF to world
     try {
@@ -266,6 +277,8 @@ void RandomWalkNode::timerCallback() {
         this->current_location = transform_stamped.transform;
         if (!this->received_first_robot_tf) {
             this->received_first_robot_tf = true;
+            this->last_location = this->current_location;
+            this->last_position_change = this->now();
             RCLCPP_INFO(this->get_logger(), "Received first robot_tf");
         }
     } catch (tf2::TransformException & ex) {
@@ -279,10 +292,12 @@ void RandomWalkNode::timerCallback() {
             }
             this->publish_plan();
             this->is_path_executing = true;
+            this->last_position_change = this->now(); // Reset stall timer when starting new plan
         } else {
             RCLCPP_INFO_ONCE(this->get_logger(), "Waiting for map and robot tf to be received...");
         }
     } else if (this->enable_random_walk && this->is_path_executing) {
+        // check if the robot has reached the goal point
         std::tuple<float, float, float> current_point = std::make_tuple(
             this->current_location.translation.x, this->current_location.translation.y,
             this->current_location.translation.z);
@@ -294,6 +309,27 @@ void RandomWalkNode::timerCallback() {
             this->is_path_executing = false;
             this->generated_paths.clear();
             RCLCPP_INFO(this->get_logger(), "Reached goal point");
+        } else {
+            // Check if position has changed significantly
+            std::tuple<float, float, float> last_point = std::make_tuple(
+                this->last_location.translation.x, 
+                this->last_location.translation.y,
+                this->last_location.translation.z);
+
+            if (get_point_distance(current_point, last_point) > this->position_change_threshold) {
+                this->last_location = this->current_location;
+                this->last_position_change = this->now();
+            } else {
+                // Check if we've been stationary for too long
+                rclcpp::Duration stall_duration = this->now() - this->last_position_change;
+                if (stall_duration.seconds() > this->stall_timeout_seconds) {
+                    RCLCPP_INFO(this->get_logger(), "Robot stationary for %f seconds, clearing plan", 
+                               stall_duration.seconds());
+                    this->is_path_executing = false;
+                    this->generated_paths.clear();
+                    this->last_position_change = this->now(); // Reset timer to avoid spam
+                }
+            }
         }
     }
 }

robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml

@@ -4,7 +4,7 @@
     map_frame: map
     robot_frame: base_link
     max_range: 10.0
-    resolution: 0.07
+    resolution: 0.5
     prob_hit: 0.99
     prob_miss: 0.1
     thres_min: 0.49