Fix/controller escape#53
Merged
Merged
Conversation
2006wu
commented
May 2, 2026
2006wu
commented
Collaborator
- Fix the problem that the robot will stop and wait when it is too close to the rival until the rival is running away.
- Controller can know the rival_robot_radius through the web. The web can adjust the radius for different opponents.
…Eurobot-2026-Navigation2 into feat/dock-omni-controller
Contributor
There was a problem hiding this comment.
Pull request overview
This PR updates docking and navigation behaviors to better handle close-range interactions (e.g., rival blocking) and to make opponent-specific radius tuning adjustable via external configuration, while also improving camera-based dock pose handling by synchronizing detection timestamps with robot pose.
Changes:
- Added timestamp-based pose synchronization / caching for camera-detected dock poses in
SimpleChargingDock. - Introduced an omni-directional “Forklift” docking controller mode with dock-side selection via
/robot/dock_side. - Reworked
TebControllerrival-handling to use an external YAML-sourced rival radius and trigger an “escape” navigation behavior, plus updated related BT / params / tooling.
Reviewed changes
Copilot reviewed 18 out of 18 changed files in this pull request and generated 8 comments.
Show a summary per file
| File | Description |
|---|---|
src/opennav_docking/opennav_docking/src/simple_charging_dock.cpp |
Adds pose queues + timer to sync detected dock pose with /final_pose, caches refined pose for getRefinedPose() |
src/opennav_docking/opennav_docking/include/opennav_docking/simple_charging_dock.hpp |
Declares pose-sync state, timer, queues, and related parameters/constants |
src/opennav_docking/opennav_docking/src/controller.cpp |
Adds dock-side subscription and “Forklift” omni docking velocity computation |
src/opennav_docking/opennav_docking/include/opennav_docking/controller.hpp |
Adds dock-side subscription member + omni docking parameters and helper declaration |
src/opennav_docking/opennav_docking/src/nav_type_selector.cpp |
Maps forklift mode token to controller function Forklift |
src/opennav_docking/opennav_docking/dock_records/scripts/plot_dock_pose_history.py |
Updates CSV filename pattern and segmentation behavior; adds last-row drop |
src/navigation2_run/params/nav2_params_default.yaml |
Adds rival-related TEB params + docking pose sync params; updates dock history filename |
src/navigation2_run/params/nav2_params_14.yaml |
Adds docking pose sync params + updates dock history filename |
src/navigation2_run/params/nav2_params_13.yaml |
Updates planner/controller tuning, rival parameters, docking pose sync params, omni docking params |
src/navigation2_run/params/nav2_params_11.yaml |
Updates BT plugins list, controller tuning, adds sima_layer, docking pose sync + omni docking params |
src/custom_controller/src/teb_controller.cpp |
Adds external rival YAML reading + rival escape behavior + /goal_reached publication |
src/custom_controller/include/teb_controller/teb_controller.hpp |
Adds action client / state for escape mode and new rival handling helpers/params |
src/custom_controller/package.xml |
Adds missing runtime dependencies for new action/YAML/TF usage |
src/custom_controller/CMakeLists.txt |
Adds new find_package deps and links yaml-cpp; updates dependency lists |
src/custom_bts/custom_nav_to_pose/behavior_trees/navigate_to_pose_w_selector.xml |
Switches recovery control flow to PersistentSequenceWContOnFail |
src/Navigation2/nav2_controller/src/controller_server.cpp |
Removes special-case handling of an old TEB rival-timeout exception |
docker/deploy/settings/.vimrc |
Adds a Vim configuration to the deploy image |
docker/deploy/Dockerfile |
Copies .vimrc into the deploy container home directory |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+144
to
+148
| print("new cluster by time") | ||
| new_segment = True | ||
| elif all_zero_time: | ||
| jump = math.hypot(x_vals[i] - x_vals[i - 1], y_vals[i] - y_vals[i - 1]) | ||
| print("new cluster by dist") |
|
|
||
| candidates = sorted( | ||
| target.glob("dock_pose_history*.csv"), | ||
| target.glob("dph*.csv"), |
| nav2_util::declare_parameter_if_not_declared( | ||
| node_, name + ".pose_match_max_delta_ms", rclcpp::ParameterValue(100)); | ||
| nav2_util::declare_parameter_if_not_declared( | ||
| node_, name + ".pose_sync_timer_period_ms", rclcpp::ParameterValue(20)); |
Comment on lines
+68
to
71
| tf2_geometry_msgs | ||
| ) | ||
| ament_target_dependencies(teb_controller | ||
| rclcpp |
Comment on lines
901
to
903
| std::scoped_lock lk(mtx_); | ||
| updateRivalStopDistance(); | ||
|
|
Comment on lines
+776
to
+805
| void TebController::updateRivalStopDistance() | ||
| { | ||
| if (!external_rival_data_path_.empty()) { | ||
| try { | ||
| YAML::Node config = YAML::LoadFile(external_rival_data_path_); | ||
| if (config["nav_rival_parameters"] && | ||
| config["nav_rival_parameters"]["rival_inscribed_radius"]) | ||
| { | ||
| rival_stop_distance_ = | ||
| config["nav_rival_parameters"]["rival_inscribed_radius"].as<double>() + | ||
| kRivalStopDistanceMargin; | ||
| rival_stop_distance_ = std::max(0.0, rival_stop_distance_); | ||
| if (rival_stop_distance_prev_ != rival_stop_distance_) { | ||
| RCLCPP_WARN( | ||
| logger_, | ||
| "[%s] rival_stop_distance updated to %f", | ||
| name_.c_str(), | ||
| rival_stop_distance_); | ||
| } | ||
| } else { | ||
| RCLCPP_WARN( | ||
| logger_, | ||
| "rival_inscribed_radius not found in YAML file, using default value"); | ||
| } | ||
| } catch (const std::exception & e) { | ||
| RCLCPP_ERROR( | ||
| logger_, | ||
| "Failed to load YAML file: %s, using default value", | ||
| e.what()); | ||
| } |
Comment on lines
217
to
+221
| obstacle_cost_threshold: 240.0 | ||
| obstacle_cost_threshold: 150.0 | ||
| rival_slowdown_dist: 1.0 | ||
| rival_min_speed_scale: 0.75 | ||
| rival_close_distance: 0.6 |
Comment on lines
254
to
+255
| obstacle_cost_threshold: 252.0 | ||
| obstacle_cost_threshold: 150.0 |
JustinShih0918
approved these changes
May 3, 2026
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.