diff --git a/docs/robot/autonomy/3_local/planning/index.md b/docs/robot/autonomy/3_local/planning/index.md index 8f2f4c6cf..a59789807 100644 --- a/docs/robot/autonomy/3_local/planning/index.md +++ b/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 \ No newline at end of file +We plan for this baseline to be DROAN. \ No newline at end of file diff --git a/mkdocs.yml b/mkdocs.yml index f6dc86d68..9fc0fe6cc 100644 --- a/mkdocs.yml +++ b/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 diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/README.md b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph/README.md new file mode 100644 index 000000000..fd2784da1 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/README.md b/robot/ros_ws/src/autonomy/3_local/a_world_models/disparity_graph_cost_map/README.md new file mode 100644 index 000000000..dd477e3b4 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/README.md b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/README.md new file mode 100644 index 000000000..c047185ca --- /dev/null +++ b/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. \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml index fcca22c32..a65daa22d 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/config/droan.yaml +++ b/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 \ No newline at end of file + custom_waypoint_distance_threshold: 0.5 + + # Disparity Graph parameters + graph_size: 5 \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp b/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp index c59d6843c..dc81ac209 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/droan_local_planner/include/droan_local_planner/droan_local_planner.hpp +++ b/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::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"); } diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md index ad4593933..9223a4a9f 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/README.md +++ b/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. diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml b/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml index 8b80240e0..5b1118664 100644 --- a/robot/ros_ws/src/autonomy/3_local/b_planners/takeoff_landing_planner/config/takeoff_landing_planner.yaml +++ b/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 diff --git a/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/README.md b/robot/ros_ws/src/autonomy/3_local/b_planners/trajectory_library/README.md new file mode 100644 index 000000000..dad899195 --- /dev/null +++ b/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. \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml index ed55b26d3..2d00b94f5 100644 --- a/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml +++ b/robot/ros_ws/src/autonomy/3_local/local_bringup/launch/local.launch.xml @@ -57,13 +57,13 @@ output="screen"> - + - + diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md index d286a46c1..5fe2e387b 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/README.md +++ b/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 diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml index b22a491b7..118d6751d 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/config/random_walk_config.yaml +++ b/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 \ No newline at end of file diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp index 00520befe..d87efa07d 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_logic.hpp +++ b/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 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 voxel_size_m; diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp index 69c983773..21e70d54d 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/include/random_walk_node.hpp +++ b/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); diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/random_walk.png b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/random_walk.png new file mode 100644 index 000000000..1efba9767 Binary files /dev/null and b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/random_walk.png differ diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp index c7299f224..685e3f1ea 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_logic.cpp +++ b/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 RandomWalkPlanner::generate_straight_rand_path( @@ -110,6 +110,14 @@ bool RandomWalkPlanner::check_if_collided_single_voxel( bool RandomWalkPlanner::check_if_collided(const std::tuple& 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 lock(this->mutex); for (const std::tuple& voxel : this->voxel_points) { @@ -144,6 +152,7 @@ std::tuple 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 rand_point(rand_x, rand_y, rand_z); std::tuple 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 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_) diff --git a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp b/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp index 3acb4e592..df6bbd5dd 100644 --- a/robot/ros_ws/src/autonomy/4_global/b_planners/random_walk/src/random_walk_node.cpp +++ b/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 RandomWalkNode::readParameters() { RCLCPP_ERROR(this->get_logger(), "Cannot read parameter: path_end_threshold_m"); return std::optional{}; } - this->declare_parameter("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("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{}; + } + this->declare_parameter("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{}; + } + this->declare_parameter("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{}; } 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 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 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 + } + } } } } diff --git a/robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml b/robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml index dc9cfe09f..e8d4b868e 100644 --- a/robot/ros_ws/src/autonomy/4_global/global_bringup/config/vdb_params.yaml +++ b/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 diff --git a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz index 9c3b161ad..5479e91b8 100644 --- a/robot/ros_ws/src/robot_bringup/rviz/robot.rviz +++ b/robot/ros_ws/src/robot_bringup/rviz/robot.rviz @@ -8,6 +8,8 @@ Panels: - /Sensors1 - /Local1 - /Local1/DROAN1 + - /Local1/DROAN1/Trimmed Global Plan for DROAN1 + - /Local1/DROAN1/Trimmed Global Plan for DROAN1/Topic1 - /Local1/Trajectory Controller1 - /Global1 Splitter Ratio: 0.590062141418457 @@ -101,8 +103,6 @@ Visualization Manager: world: map_FLU: base_link_ground_truth: - {} - map: base_link: base_link_frd: {} @@ -113,6 +113,7 @@ Visualization Manager: {} ouster: {} + map: base_link_stabilized: {} look_ahead_point: @@ -262,17 +263,17 @@ Visualization Manager: Value: /robot_1/droan/disparity_map_debug Value: false - Class: rviz_default_plugins/MarkerArray - Enabled: true + Enabled: false Name: Disparity Graph Poses Namespaces: - pose: true + {} Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: /robot_1/droan/disparity_graph - Value: true + Value: false - Class: rviz_default_plugins/MarkerArray Enabled: true Name: Trimmed Global Plan for DROAN @@ -360,17 +361,17 @@ Visualization Manager: Value: trajectory_controller/trajectory_vis Value: true - Class: rviz_default_plugins/MarkerArray - Enabled: false + Enabled: true Name: Traj Debug Namespaces: - {} + default: true Topic: Depth: 5 Durability Policy: Volatile History Policy: Keep Last Reliability Policy: Reliable Value: trajectory_controller/trajectory_controller_debug_markers - Value: false + Value: true Enabled: true Name: Trajectory Controller Enabled: true @@ -466,25 +467,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 21.319120407104492 + Distance: 18.401203155517578 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 1.0440632104873657 - Y: 1.6269922256469727 - Z: 0.25401341915130615 + X: -1.8554298877716064 + Y: 5.675510883331299 + Z: -2.8006136417388916 Focal Shape Fixed Size: false Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.390395849943161 + Pitch: 0.4553954601287842 Target Frame: Value: Orbit (rviz) - Yaw: 0.8517506122589111 + Yaw: 0.07311739027500153 Saved: ~ Window Geometry: Displays: