Comments (4)
Where is the goal in this example and what are your goal tolerances? I've never seen one quite so involved, kudos to you!
Interestingly, a change like proposed in #4003 would solve this issue for you. It also isn't terribly difficult or involved either to implement. I think @jwallace42 might be working on it, but we should check with him first to make sure -- if not, that would be a great contribution to the stack and something we can consider strongly making a default behavoral change for Nav2 out of the box (@naiveHobo ). This seems like a reasonable, general purpose change to make across the board.
if close to goal -> check if path is valid -> if so, don't update
EZ-PZ with intuitive parameter dials to tune for any given situation
from navigation2.
I am currently not looking into it @shannonwarren so you are free to tackle it. :)
from navigation2.
My goal point is exactly where the purple marker is located (lookahead point) and my goal tolerances are
course_goal_checker:
plugin: "nav2_controller::SimpleGoalChecker"
xy_goal_tolerance: 0.25
yaw_goal_tolerance: 0.2
Thank for the above information. We're facing the similar looping issue.
I'm looking into your suggestion of implementing a proximity check and avoid replanning. My current thought process is to use the PathExpiringTimer condition since that triggers new paths periodically and adding an input port which takes in the goal pose.
Condition ID="PathExpiringTimer">
<input_port name="seconds">Time to check if expired</input_port>
<input_port name="path">Check if path has been updated to enable timer reset</input_port>
<input_port name="goal">Destination</input_port>
and then in the pathexpiringtimer bt node doing a simple check.
if (isRobotNearGoal()) {
return BT::NodeStatus::FAILURE;
}
What do you think? @SteveMacenski @jwallace42
Additionally, increasing the <PathExpiringTimer seconds="60" path="{path}" goal="{goal}"/>
to a really large value kinda helps (makes senses but doesnt solve the problem but helps unnecessary replanning). Are there any downsides to this approach that I'm not thinking about?
Just as a reference this is my behavior tree
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<RecoveryNode number_of_retries="6" name="NavigateRecovery">
<PipelineSequence name="NavigateWithReplanning">
<PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
<RateController hz="1.0">
<RecoveryNode number_of_retries="1" name="ComputePathToPose">
<Fallback>
<ReactiveSequence>
<Inverter>
<PathExpiringTimer seconds="60" path="{path}" goal="{goal}"/>
</Inverter>
<Inverter>
<GlobalUpdatedGoal/>
</Inverter>
<IsPathValid path="{path}"/>
</ReactiveSequence>
<ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}"/>
</Fallback>
<ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
</RecoveryNode>
</RateController>
<GoalCheckerSelector selected_goal_checker="{selected_goal_checker}" default_goal_checker="course_goal_checker" topic_name="goal_checker_selector"/>
<ControllerSelector selected_controller="{selected_controller}" default_controller="course_navigation" topic_name="controller_selector"/>
<ReactiveSequence name="MonitorAndFollowPath">
<PathLongerOnApproach path="{path}" prox_len="5.0" length_factor="1.1">
<RetryUntilSuccessful num_attempts="1">
<SequenceStar name="CancelingControlAndWait">
<CancelControl name="ControlCancel"/>
<Wait wait_duration="10"/>
</SequenceStar>
</RetryUntilSuccessful>
</PathLongerOnApproach>
<RecoveryNode number_of_retries="1" name="FollowPath">
<FollowPath path="{path}" controller_id="{selected_controller}" goal_checker_id="{selected_goal_checker}"/>
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
<BackUp backup_dist="0.30" backup_speed="0.05"/>
<Wait wait_duration="2"/>
<Spin spin_dist="-0.1"/>
<DriveOnHeading dist_to_travel="0.60" speed="0.05"/>
</Sequence>
</RecoveryNode>
</ReactiveSequence>
</PipelineSequence>
<ReactiveFallback name="RecoveryFallback">
<GoalUpdated/>
<RoundRobin name="RecoveryActions">
<Sequence name="ClearingActions">
<ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
<ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
</Sequence>
<BackUp backup_dist="0.30" backup_speed="0.05"/>
<Wait wait_duration="2"/>
<!-- <Spin spin_dist="-0.1"/> -->
<!-- <DriveOnHeading dist_to_travel="0.50" speed="0.05"/> -->
<Strafe strafe_dist="0.10" strafe_speed="0.05"/>
</RoundRobin>
</ReactiveFallback>
</RecoveryNode>
</BehaviorTree>
</root>
from navigation2.
0.25/0.20 aren't that tight of goal criteria to be a contributor.
I think perhaps we should move this discussion over to that ticket if we're going down that direction. Thus closing this ticket and moving my response over there
from navigation2.
Related Issues (20)
- heap-buffer-overflow bug caused by user misconfiguration (amcl:min_particles=a negative value) HOT 2
- Removing AMCL from nav2_bringup launch HOT 2
- Error codes in NavigateToPose/NavigateThroughPoses
- Hi, I am building the package of nav2_waypoint_follower and facing that errors, HOT 2
- New "Configure" transition for the LifecycleManager HOT 2
- Accessing waypoints inside a controller plugin HOT 3
- Fix flaky spin test
- Nav2 Stalling on Multiple Robots? HOT 9
- Robot using nav2 begins pathfollowing but never finishes HOT 3
- MPPI cannot follow global path accurately HOT 1
- [Collision_monitor] Approach polygon time=0 step is not processed HOT 7
- The lidar point cloud of NAV2 shifted significantly after being stationary for 5 minutes. HOT 5
- Errors with controller_server using GPS for Navigation HOT 4
- Obstacle Position Shift in Map after Loading HOT 2
- Full footprint collsion distance in MPPI obstacle critic and use of collision_margin_distance. HOT 12
- local_costmap does not respect the use_sim_time parameter HOT 12
- Vector polygon - Collision Monitor is not available for ROS2 humble HOT 5
- [collision_monitor] Add temporal axis to min_points behavior HOT 2
- [ERROR] [1716697697.080277840] [rviz2]: Lookup would require extrapolation into the future. Requested time 1716697697.040514 but the latest data is at time 1716697697.039929, when looking up transform from frame [laser_frame] to frame [odom]
- Laser Scan rotates with robot ------- [ERROR] [1716697697.080277840] [rviz2]: Lookup would require extrapolation into the future. Requested time 1716697697.040514 but the latest data is at time 1716697697.039929, when looking up transform from frame [laser_frame] to frame [odom] HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from navigation2.