Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[DWB] PathAlign and GoalAlign critics lead to poor obstacle avoidance #938

Closed
bpwilcox opened this issue Jul 16, 2019 · 16 comments · Fixed by #1747
Closed

[DWB] PathAlign and GoalAlign critics lead to poor obstacle avoidance #938

bpwilcox opened this issue Jul 16, 2019 · 16 comments · Fixed by #1747
Labels
2 - Medium Medium Priority help wanted Extra attention is needed
Projects

Comments

@bpwilcox
Copy link
Collaborator

bpwilcox commented Jul 16, 2019

The PathAlign and GoalAlign critics appear to bias the dwb_controller path following results to steer the robot dangerously close to obstacles, particularly seen when rounding corners. Removing them (set scale to 0) appears to lead to better performance. There is likely some bug either with the critics themselves or as a consequence of something else that is happening (perhaps the pruning of the goal position?). We should investigate these critics and test that they are being enacted properly.

Steps to reproduce issue

- launch navigation bringup
- set pose
- navigate to goal

Expected behavior

  • should reach goal while avoiding obstacles consistently when there is ample space to avoid them

Actual behavior

  • moves very close to obstacle posts when rounding corners, often failing with DWB even when the global plan is far from obstacles and there is ample obstacle-free room
@crdelsey crdelsey added the 1 - High High Priority label Jul 17, 2019
@crdelsey crdelsey added this to the E Turtle Release milestone Jul 17, 2019
@crdelsey
Copy link
Collaborator

crdelsey commented Jul 17, 2019

We need to understand why this is.

@SteveMacenski SteveMacenski changed the title PathAlign and GoalAlign critics lead to poor obstacle avoidance [DWB] PathAlign and GoalAlign critics lead to poor obstacle avoidance Aug 8, 2019
@crdelsey crdelsey removed this from the E Turtle Release milestone Oct 28, 2019
@SteveMacenski SteveMacenski added 2 - Medium Medium Priority and removed 1 - High High Priority labels Nov 21, 2019
@SteveMacenski SteveMacenski added this to To do in V1.0.0 via automation Jan 31, 2020
@SteveMacenski SteveMacenski moved this from To do Features to To do Bugs in V1.0.0 Jan 31, 2020
@SteveMacenski
Copy link
Member

SteveMacenski commented Feb 15, 2020

@DLu if you’re interested in touching base here- this ticket would be a natural starting point 😉

@SteveMacenski SteveMacenski added the help wanted Extra attention is needed label Mar 11, 2020
@shrijitsingh99
Copy link
Contributor

shrijitsingh99 commented Apr 28, 2020

I encountered a similar issue where the robot would get stuck in sharp turns especially u-turns.
After some digging, it seems as if the robot gets stuck in a sort of local minima as theGoalDistCritic assigns a cost based on euclidean distance from the goal/part of the global plan at the edge of local costmap. So the minimum cost is just going through the wall, not around the obstacle.
Below is a visualization of the GoalDistCritic costs:

Screenshot from 2020-04-29 01-42-01 copy

So to avoid these minima, cost should be set based on euclidean distance from point on the global path which is at a distance x from current position, where
x = (current_speed * acc_lim_trans_ * (1 / controller_frequency_)) * sim_time_ + minimum_threshold (if TrajectoryGenerator is LimitedAccelGenerator).

Below is the visualization of the modifiedGoalDistCritic costs when the bot is running with speeds of around 0.4m/s:

Screenshot from 2020-04-29 01-44-41

When the turn comes:

Screenshot from 2020-04-29 01-45-44

@SteveMacenski
Copy link
Member

SteveMacenski commented Apr 28, 2020

Shouldn't you just lower the weight on goal distance so it doesn't overpower the others for going around obstacles? It sounds like you have it cranked in one direction really hard.

After some digging, it seems as if the robot gets stuck in a sort of local minima as theGoalDistCritic assigns a cost based on euclidean distance from the goal/part of the global plan at the edge of local costmap

So to avoid these minima, cost should be set based on euclidean distance from point on the global path which is at a distance x from current position, where

These two statements are both very unclear to me what you mean. Can you clarify?

@SteveMacenski
Copy link
Member

SteveMacenski commented Apr 28, 2020

Also, this doesn't seem related to the ticket above. Is this another thing you're talking about?

@shrijitsingh99
Copy link
Contributor

shrijitsingh99 commented Apr 29, 2020

Shouldn't you just lower the weight on goal distance so it doesn't overpower the others for going around obstacles? It sounds like you have it cranked in one direction really hard.

So if you lower the weight for goal distance the robot gets stuck at its current position as there is no pulling factor driving it towards the goal.

These two statements are both very unclear to me what you mean. Can you clarify?

Essentially the robot gets stuck in the below case as the goal distance is not pulling it in the forward direction instead it is pulling it right.

Screenshot from 2020-04-29 01-42-01 copy

One solution to this problem is to enable prune_plan but the problem in that is prune_distance is a fixed threshold so if set to too small value the max velocity the robot is limited by it.
My idea was somewhat to dynamically adjust the prune_dist inside the GoalDistCritic based on the robot's current velocity.

Also, this doesn't seem related to the ticket above. Is this another thing you're talking about?

The working of GoalDist and GoalAlign align critic is pretty much the same as the share the same cost grid. So the working of GoalAlign is quite affected by the cost grid generated in GoalDist.
One more way they are related is that setting a small prune_distance will lead to similar behavior as mentioned in OP, because the robot will try to shortcut the global plan by going near obstacles (which yield shorter paths) during turns.

@SteveMacenski
Copy link
Member

SteveMacenski commented Apr 29, 2020

So if you lower the weight for goal distance the robot gets stuck at its current position as there is no pulling factor driving it towards the goal.

Isn't that path distance critic?

You should 100% be pruning plans unless you have a compelling reason not to. Frankly, I don't know what the specialty reason not to prune even is. If you know, do inform me.

Again, I am not clear what this has to do with the ticket above. Is this related to the PathAlign and GoalAlign critics leading to poor obstacle avoidance performance? I'm not drawing a connection from this discussion.

I'm re-reading your comments and I'm not seeing a question, action item, or otherwise clarification. It looks like you're tuning the gains of these parameters. I'm not sure the behavior you're describing is at all a bug but a tuning problem. Can you please be very clear about what your problem is, what your solving for, and what your solution is.

@DLu can you help at all with this? My familiarity with the specific interactions within DWB is low.

@jackpien
Copy link
Contributor

jackpien commented May 11, 2020

I am able to reproduce this somewhat deterministically with the following steps:

  1. In nav2_params.yaml set:
    1. FollowPath.PathAlign.scale: 32.0
    2. FollowPath.GoalAlign.scale: 24.0
  2. Launch the tb3_simulation
  3. Set the initial pose:
ros2 topic pub -1 /initialpose geometry_msgs/msg/PoseWithCovarianceStamped "
{
header: {
  stamp: {
    sec: 1588272932,
    nanosec: 798746418,
	},
  frame_id: map
  },
pose: {
  pose: {
    position: {
      x: -2.0232667922973633,
      y: -0.5306379795074463,
      z: 0.0
	  },
    orientation: {
      x: 0.0,
      y: 0.0,
      z: 0.0,
      w: 1.0
	  }
	},
  covariance: [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]
  }
}
"
  1. Set the goal pose:
ros2 action send_goal /navigate_to_pose nav2_msgs/action/NavigateToPose "
{
pose: {
  header: {
    stamp: {
      sec: 1588272932,
      nanosec: 798746418,
	  },
    frame_id: map
    },
  pose: {
    position: {
      x: -0.54,
      y: 0.18,
      z: 0.0
      },
    orientation: {
      x: 0.0,
      y: 0.0,
      z: 0.0,
      w: 1.0
      }
    }
  }
}
"

Running 9 out of 10 times, you should see the TB3 get "jammed" against a pillar, while moving towards the goal, and cause an "No valid trajectories found" error.


This "jamming" occurs with either GoalAlign or PathAlign enabled (ie scale != 0.0). Seems to be worse with GoalAlign enabled. With just PathAlign enabled, it "jams" maybe 1 out of 10 runs but otherwise precariously hugs the pillar as it moves successfully towards the goal.

When scoring a trajectory, both GoalAlign and PathAlign forward transform the current pose by a fixed forward_point_distance which defaults to 0.325. Otherwise, the scoring is equivalent to GoalDist and PathDist.

During the initial GoalAlign->prepare(...) there's a transformation for the final goal pose in the truncated global_plan (clipped to be inside the dynamic window?) that looks a little suspect (and I don't really understand the comments in the code). Basically we compute the angle between the current pose and the final goal pose. Then we take the last pose in "global_plan" (which is not really the final goal pose since "global_plan" looks truncated from the "real" global plan - clipped to be within the dynamic window?) - and forward projects that last pose towards that angle by the same forward_point_distance 0.325 meters. This projection can cause the last pose in the "truncated" global plan to be seemingly projected out into an arbitrary location, definitely not guaranteed to be aligned to the goal depending on the current pose. Otherwise, the costmap preparation uses the same code as GoalDist.


Things I tried:

  1. Setting GoalAlign.forward_point_distance and PathAlign.forward_point_distance to 0.1 which drastically improved the stability, removes the jam I see. A hacky fix for this issue would be to use 0.1 as the forward_point_distance value set in nav2_params.yaml with Goal and PathAlign enabled.

  2. In GoalAlign->prepare(...), removed the forward projection of the last pose from the "truncated" global plan and just use the last pose as-is. Seemed to allow the TB3 to follow the global plan a bit more predictably. This seems to be intuitively the correct thing to do. It favors a trajectory that aligns to the final goal pose in the "truncated" global_plan.


Things I have not tried:

  1. Dive deeper to understand why we get into a "No valid trajectories found" error situation.
  2. In GoalAlign->prepare(...) use the angle between the last pose from "global_plan" and the real goal pose to project the last pose.
  3. Messing with the 32 and 24.0 scales.

Questions I have (given I'm a DWB noob and still trying to understand the nav code in general):

  1. How were the 32.0 and 24.0 scale values for the Goal and PathAlign/Dist critics chosen?
  2. In the GoalAlign->prepare(...) func, is last pose projection code correct (fwiw, the code is the same as ROS1 locus nav repo)?
  3. Is it possible for the DWB local planner to select a trajectory that eventually causes the TB3 to crash into the pillar? Where in the code do trajectories that lead into obstacles get trivially rejected? Maybe the trajectory_generator does this?
  4. Even with poorly tuned forward_point_distance variable, should that lead to TB3 crashing into the wall? Or should the local_planner be able to compensate and still publish safe velocities (although they might not be totally efficient paths)?

Suggestions? Other thoughts?

@SteveMacenski
Copy link
Member

SteveMacenski commented May 11, 2020

Thank you for taking this on and such good setup for reproduction. Can you post a gif of the robot in this "jam" position so there's some visual understanding of what you mean?

@shrijitsingh99 also was looking into DWB behavior, I think he should have some helpful comments on this. @shrijitsingh99 can you comment on the GoalAlign->prepare(...) paragraph? That sounds related to what you were looking into.

A hacky fix for this issue would be to use 0.1 as the forward_point_distance value set in nav2_params.yaml with Goal and PathAlign enabled.

Why is that a hacky fix? Given that this is for just the align critic and not the distance critic / a carrot, that seems reasonable to me. I would be vaguely interested if you used another robot (ex rover since its already in ROS2, usually I'd say Jackal) that's larger if you see the same behavior. Maybe its b/c the TB3 is small?

How were the 32.0 and 24.0 scale values for the Goal and PathAlign/Dist critics chosen?

Heuristically, these are hyper parameters so they're not rooted in math, they're turned by hand to get performance an engineer deems good. Those are probably the params Locus uses if that is the default they set.

In the GoalAlign->prepare(...) func, is last pose projection code correct (fwiw, the code is the same as ROS1 locus nav repo)?

I think you bring up a real potential issue, and the answer is that I'm not sure. Can you post a ticket on the robot_navigation repo and maybe David will answer with your explanation. I would agree at first glance that that should be the real end of path not the end of the window, but I could see that also being the case as part of the "dynamic window" method.

Is it possible for the DWB local planner to select a trajectory that eventually causes the TB3 to crash into the pillar?

I think that's the role of the footprint critic, as its forward projecting if in collision it should score poorly.

Even with poorly tuned forward_point_distance variable, should that lead to TB3 crashing into the wall

Not 100% sure. Its all a scheduling problem so you could schedule yourself right into a wall if you choose hyper parameters that encourage behavior like that, but I wouldn't think the forward_point_distance would be that parameter, but rather the weights.

@jackpien
Copy link
Contributor

jackpien commented May 12, 2020

Thanks @SteveMacenski for the thorough feedback.

Maybe its b/c the TB3 is small?

That was my initial thought which is why I adjusted the forward_point_distance down. I figured whatever Locus is using is much larger, at least 2x larger than TB, if not larger.


I'll put the following on my things-to-do list:

  1. Post a gif in this issue of the "jam" I see with GoalAlign and PathAlign enabled (default forward_point_distance == 0.325).

  2. Create a pull request to "fix" this issue by setting forward_point_distance to 0.1 for nav2_params.

    1. I think this is hacky because ideally, I think badly tuned heuristics should cause inefficiencies but it shouldn't cause the system to blow up (ie TB3 to crash into the pillar). The right fix I believe has to do with rejecting any trajectory that will inevitably lead into obstacles given the max acceleration / deceleration parameters of the robot (huh, maybe those parameters in nav2_params are not correct for the TB3??).
  3. Will post an issue / ticket in the robot_navigation repo about GoalAlign->prepare(..) to understand what's going on with the goal pose projection - to see if David, etal have feedback on that.

Anything else / other comments (specifically around boldfaced bubble thought in 2-i)? Otherwise stay tuned...

@SteveMacenski
Copy link
Member

SteveMacenski commented May 12, 2020

2i, maybe try that out? I know that in the nav2_params we don't fully describe all the parameters for everything.

I would though like Shrijit's input.

@cdelsey
Copy link
Contributor

cdelsey commented May 12, 2020

Is it possible for the DWB local planner to select a trajectory that eventually causes the TB3 to crash into the pillar? Where in the code do trajectories that lead into obstacles get trivially rejected? Maybe the trajectory_generator does this?

As Steve said, these are rejected by the obstacle critics. Either BaseObstacle or ObstacleFootprint

Even with poorly tuned forward_point_distance variable, should that lead to TB3 crashing into the wall? Or should the local_planner be able to compensate and still publish safe velocities (although they might not be totally efficient paths)?

In a perfect world, it should be impossible. However, I believe the *Align critcs encourage the robot to take a marginal path, and then either sensor noise or localization noise causes it to think it is on a obstacle. At that point it fails.

There is a topic that DWB publishes - evaluation. This outputs the scores of each critic at every step of the DWB planner. If you can get a rosbag of a failing scenario, you might be able to go back and look at all the scores that led to the failure.

@shrijitsingh99
Copy link
Contributor

shrijitsingh99 commented May 12, 2020

During the initial GoalAlign->prepare(...) .. definitely not guaranteed to be aligned to the goal depending on the current pose. Otherwise, the costmap preparation uses the same code as GoalDist.

From what I had understood, it determines the angle of the slope made by the line joining the current position and position. Then it projects the last pose of the pruned global plan using this calculated angle and forward point distance.
So essentially, it serves as a guiding factor in aligning the robots towards the general heading of the goal. If you look at the cost grid, you can see it that the it sort of guides it towards the go.

It's not trying to perfectly align with the goal, it gives a sort of general heading. IMO you should never try to align to the perfect with the goal position unless you are near the goal.

Because of the arbitrary location and its tendency to favor trajectories directed in the heading of the goal even if in that heading there is an obstacle.

Setting GoalAlign.forward_point_distance and PathAlign.forward_point_distance to 0.1

By setting it to a small value you essentially reduce the GoalAlign critic to GoalDist critic so might be why it solves the issue. But I do agree that for turtle bot this value should be the radius of the robot.

One thing you should also do is increase the BaseObstacle scale as GoalAlign is quite similar to GoalDist so by setting both of their scales to '24.0' you have essentially double the scale for GoalAlign so you should also increase the BaseObstale scale., though that might lead to the robot getting stuck (due to sort of minima) but it will not crash.

  1. In GoalAlign->prepare(...), removed the forward projection of the last pose from the "truncated" global plan and just use the last pose as-is. It seemed to allow the TB3 to follow the global plan a bit more predictably. This seems to be intuitively the correct thing to do. It favors a trajectory that aligns to the final goal pose in the "truncated" global_plan.

This is not at all trying to align with the goal or even align in the heading of the goal. You are trying to align with the last pose of the truncate global plan, which is not what this plugin intended.

  1. Dive deeper to understand why we get into a "No valid trajectories found" error situation.

This is a good idea. Though mostly it is due to the robot crashing or oscillation from what I have noticed.

The right fix I believe has to do with rejecting any trajectory that will inevitably lead into obstacles given the max acceleration / deceleration parameters of the robot
It will always reject such trajectories, the reason it crashes that due to the critics and their scales it drives very close to the robot and due to sensor/localization noise or reaction time of the robot will end up on lethal cost as mentioned by @cdelsey

I think I know the reason behind which is that on turns with obstacles these goal critics and alignment critics essentially cause the robot to not follow the path and try to take a shortcut which
will lead close to the obstacle and causing it to crash. I haven't really been able to explain it properly till now nor have I got the time till now to explore further though I think this is the issue.

The question though remains can this problem be overcome by tuning or is it an issue with the critics. Since @SteveMacenski has mentioned that Locus uses this on their robots and their robots do not face this issue so this is most likely to be a tuning issue.

@SteveMacenski
Copy link
Member

SteveMacenski commented May 12, 2020

It sounds like pushing the .3125 to something smaller is generally agreed to be "good", lets do that. Also another place where explicit documentation is good . . .

@jackpien does that fix most of the problems for the alignment critics or still more to explore?

@jackpien
Copy link
Contributor

jackpien commented May 17, 2020

Thanks all for the feedback.

Post a gif in this issue of the "jam" I see with GoalAlign and PathAlign enabled (default forward_point_distance == 0.325).

Attached is an image of the "jam" I see when running the repro steps I mentioned above.

Screenshot from 2020-05-16 18-01-55


Create a pull request to "fix" this issue by setting forward_point_distance to 0.1 for nav2_params.

I made a PR that enables the Goal/PathAlign critics and reduces the forward_point_distance for those critics.

In summary to everyone's comment, it sounds like mis-tuned parameters can lead to "jams" due to system noise. But there may be other issues to look at (which I described in the PR comments).


Will post an issue / ticket in the robot_navigation repo about GoalAlign->prepare(..) to understand what's going on with the goal pose projection - to see if David, etal have feedback on that.

@shrijitsingh99 it sounds like you think the code is correct. So I will hold off on creating an issue in robot_navigation then.

V1.0.0 automation moved this from To do Bugs to Done May 18, 2020
SteveMacenski pushed a commit that referenced this issue May 18, 2020
The forward_point_distance used in the GoalAlign and PathAlign critics projects the current pose forward a default 0.325 meters before scoring the trajectory. This can cause velocities that create sharp turns into obstacles to be chosen (reproducible with the Turtlebot3 simulation).

For TB3, which is small in size, 0.325 meters is too far. Instead, enable GoalAlign and PathAlign critics in the DWB controller and shorten the critics' forward_point_distance (how far the current pose is project given the current orientation before scoring the trajectory) from the default 0.325 meters to 0.1 meters which improves stability when running with TB3.

Signed-off-by: Jack Pien <jack@jackpien.com>
SteveMacenski added a commit to SteveMacenski/navigation2 that referenced this issue Jun 17, 2020
* Corrected check to detect collision (ros-planning#1404) (ros-planning#1601)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Script (ros-planning#1599)

* Add eloquent option to build script and made it default

* Add eloquent in the error message

* Remove dashing

* setStatus(BT::NodeStatus::IDLE) removed (ros-planning#1602)

the removed code has no effect at all: the status of a node will be its returned value!.

In general, you never set your status to IDLE, unless halted.

* Spin recovery (ros-planning#1605)

* Corrected check to detect collision (ros-planning#1404)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Changed collision check to detect only lethal and unknown cells (ros-planning#1404)(ros-planning#1603)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* making documentation distro agnostic (ros-planning#1610)

* Issue 1608 segmentation fault planner server node master (ros-planning#1612)

* corrected wrong indexing in NavfnPlanner::smoothApproachToGoal function

* resolved uncrustify errors

* corrected the condition inside NavfnPlanner::smoothApproachToGoal to replace last pose of computed path

Co-authored-by: chikmagalore.thilak <chikmagalore.thilak@bshg.com>

* BehaviorTree refactoring (ros-planning#1606)

* Proposed refactoring to avoid issues with CoroAction

* correctly haltAllActions (related to ros-planning#1600)

* not really needed and will be deprecated soon

* Applying changes suggested in the comments of ros-planning#1606

- fix haltAllActions
- changes method signature on_success()
- reverts the changes made here:
https://github.com/ros-planning/navigation2/pull/1515/files

* fix warnings and errors

* make uncrustify happy?

* Update bt_navigator.cpp

* Update bt_navigator.cpp

* uncrustify fix

Co-authored-by: daf@blue-ocean-robotics.com <Davide Faconti>

* Decide the output of BtServiceNode based on the service-response (ros-planning#1615)

- `BtServiceNode::check_future()` was created, to encapsulate the logic
where a the output of the BtServiceNode is computed.
- Inherited classes can overload this function according to the requirement
of the user

* Add server_name parameter to BtActionNode (ros-planning#1616)

A BT port is introduced in case the user wants to change the default action_name
of BtActionNode.

* Added thread synchronization to KinematicParameters (ros-planning#1459) (ros-planning#1621)

* Added thread synchronization to KinematicParameters (ros-planning#1459)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Doxygen fix (ros-planning#1459)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Docs (ros-planning#1629)

* Updated website gifs (ros-planning#1228)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Created new gifs (ros-planning#1228)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Corrected gif name (ros-planning#1228)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Allow reaction to cancellation and preemption on wait recovery plugin (ros-planning#1636)

* Changed onCycleUpdate to allow preemption (ros-planning#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Deleted unnecessary wait and corrected style (ros-planning#1622)

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Nav2 planner plugin tutorial (ros-planning#1633)

* Nav2 planner plugin tutorial draft 1

* Reduced gif size

* Updated the config code block

* Remove RRTConnect plugin and add StraightLine plugin.

* Address reviewer's comments

* Add information about planners mapping in Navigation2

* Minor path fix for gif (ros-planning#1638)

* Minor improvement on best practice for pluginlib export (ros-planning#1637)

* Add caps to headers (ros-planning#1639)

* Fix segfault in test_planner_random_node/test_planner_costmaps_node (ros-planning#1640)

The problem appears while addressing zero-length path vector
after planner failed to create a plan.

* Fix trans_stopped_velocity comparison with x & y velocity (ros-planning#1649)

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* map_server refactor and cleanup (ros-planning#1624)

* [WIP] map_server refactor and cleanup

nav2_map_server/mapio (map input-optput library):
* Move OccupancyGrid messages saving code from MapSaver::mapCallback()
  to saveMapToFile() function
* Rename and move try_write_map_to_file() MapSaver method
  to tryWriteMapToFile() function
* Move map saving parameters into one SaveParameters struct
* Reorganize map saving parameters verification code from MapSaver
  to new checkSaveParameters() function
* Correct logging for incorrect input cases in checkSaveParameters()
* Copy loadMapFromYaml() method from OccupancyGridLoader
* Move loadMapFromFile() method from OccupancyGridLoader
* Rename and move load_map_yaml() OccupancyGrid method
  to loadMapYaml() function
* Move LoadParameters struct from OccupancyGridLoader

nav2_map_server/map_saver:
* Completely re-work MapSaver node:
 - Switch MapSaver from rclcpp::Node to nav2_utils::LifecycleNode
 - Revise MapSaver node parameters model
 - Add saveMapTopicToFile() method for saving map from topic
 - Remove future-promise synchronization model as unnecessary
* Add "save_map" service with new SaveMap service messages
* Rename map_saver_cli.cpp -> map_saver_cli_main.cpp file
  and map_saver -> to map_saver_cli executable
* Add ability to save a map from custom topic ("-t" cli-option)
* Restore support of "--ros-args" remappings
* Update help message in map_saver_cli
* New map_saver_server_main.cpp file and map_saver_server executable:
  continuously running server node
* New launch/map_saver_server.launch.py: map_saver_server launcher

nav2_map_server/map_server:
* Revise MapServer node parameters model
* Rename loadMapFromYaml() -> loadMapResponseFromYaml()
* Add node prefix to "map" and "load_map" service names
* Fix crash: dereferencing nullptr in map_server running as a node
  while handling of incorrect input map name
* Add updateMsgHeader() method for correcting map message header
  when it belongs to instantiated object
* Rename main.cpp -> map_server_main.cpp file
* Minor changes and renames to keep unified code style

nav2_util/map_loader:
* Remove as duplicating of loadMapFromFile() from MapIO library

other:
* Update nav2_map_server/README
* Fix testcases

* Fixes for cpplint, uncrustify, flake8 and test_occ_grid_node failures

* Fixing review comments

* Rename mapio -> map_io
* Move all OccGridLoader functionality into MapServer. Remove OccGridLoader
* Switch all thresholds to be floating-point
* Switch loadMapFromYaml() returning type to LOAD_MAP_STATUS and remove
  duplicating code from loadMapResponseFromYaml()
* Make mapCallback() to be lambda-function
* Make saveMapCallback() to be class method
* Utilize local rclcpp_node_ from LifecycleNode instead of map_listener_.
  Remove map_listener_ and got_map_msg_ variables.
* Rename load_map_callback() -> loadMapCallback() and make it to be class method
* Rename handle_occ_callback() -> getMapCallback() and make it to be class method
* Force saveMapTopicToFile() and saveMapToFile() to work with constant arguments
* map_saver_cli: move arguments parsing code into new parse_arguments() function
* Rename test_occ_grid_node -> test_map_server_node and fix test
* Rename test_occ_grid -> test_occ_grid and fix test
* Fix copyrights
* Fix comments
* Update README

* Increase test coverage

* Fixing review comments

* Separate map_server and map_saver sources
* Fix copyrights
* Suppress false-positive uncrustify failure

* Map Server docs update for Foxy (ros-planning#1650)

* Map Server docs update for Foxy

* Fixes after review

* Add brief description of map_io

* Initialize variabes to avoid errors during build (ros-planning#1654)

Signed-off-by: Pablo Vega <epvega@gmail.com>

* Temporarily remove planner tests (ros-planning#1656)

* Replace deprecated ament_export_interfaces

* Revert edf9b9a

Accidental commit into github ui

* remove docs from navigation2 repo (ros-planning#1657)

* Update hyperlinks in readme for new website docs (ros-planning#1668)

* Update hyperlinks in readme for new website docs

* Adding link to join slack

* Ignore codecov paths in tests (ros-planning#1671)

* Ignore codecov paths in tests

* adding missing string test case for stripping leading slash

* Update test_string_utils.cpp

* Update test_string_utils.cpp

* Update codecov.yml

* Update codecov.yml

* Update codecov.yml

* Update codecov.yml

* Fix bug in the condition to publish local plan (ros-planning#1674)

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

* Fix infinite rotation in Spin recovery when angle > PI (ros-planning#1670)

* Fix infinite rotation in Spin recovery when angle > PI

* Add test for spin recovery

* Fix formatting

* Add general optimizations

* Fix copyright

* Add weights to AMCL particle cloud (ros-planning#1677)

* Add Particle and ParticleCloud msgs to publish amcl particle cloud with weights

* Add deprecation warning

* Remove dead code from navfn planner (ros-planning#1682)

* adding wait recovery integration test (ros-planning#1685)

* adding recovery wait test

* adding copy rights

* fixing gaurds

* Add ignoring code cov any files named test_ (ros-planning#1691)

* Add ignoring code cov any files named test_

* Update slack URL

* Update codecov.yml

* Replace current cell reference with copy (ros-planning#1690)

The cell reference becomes invalidated as the reference becomes invalid when a new  cell is added to the vector

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Fix segfault in test_planner_random_node (ros-planning#1694)

deactivate needs to be called before cleanup
to stop the mapUpdateLoop()

Signed-off-by: Siddarth Gore <siddarth.gore@gmail.com>

* Adding more simple action server, nav2_utils, and lifecycle bringup CLI tests (ros-planning#1695)

* adding tests to simple action server and fixed bug

* more test coverage in nav2_utils

* testing lifecycle cli program

* shifting action server test around for stability

* adding test temp commented out

* flake

* w/o resetting

* Change publishers to publish unique ptrs (ros-planning#1673)

* Change publishers to publish unique ptrs

* Revert test case modification

* Update function signature to receive unique_ptrs

* Update publishers in nav2_costmap_2d to publish unique ptrs

* Update publishers in nav2_planner and nav2_map_server

* Change nav2_map_server publisher to publish occupancy grid unique ptr

* Change publisher in nav2_planner to publish path unique ptr

* Remove smart pointer return from functions in nav2_costmap_2d

* Run cpp_lint manually in nav2_costmap_2d

* Minor fixes

* Adhere to conventions of smart pointers passing to function

* Change publisher in dwb_core to publish unique pointer

* update for BT.cpp master (ros-planning#1714)

* Update CI for ROS2 Foxy (ros-planning#1684)

* Update Dockerfile

* Update repo paths

* Copy all of workspaceto include .repo files expected by CI in image

* Patch Docker and CI for missing gazebo 11
Revert once gazebo 11 is ready

* Disable connext for now

* Disable debug jobs in nightly workflow

* Remove unused install_deployment_key refrence

* Update the release Dockerfile

* Use buildkit to speed up loacal builds

* Add example of running tests

* Roll back BTcpp and no-error deprecated warnings

* Revert "Roll back BTcpp and no-error deprecated warnings"

This reverts commit 301eb54.

* Move extra Dockerfiles to tools folder

* Replace deprecated ament_export_interfaces (ros-planning#1669)

with ament_export_targets
See notice here:
https://index.ros.org/doc/ros2/Releases/Release-Foxy-Fitzroy/

* Refactor rclcpp::executor::FutureReturnCode deprecation (ros-planning#1702)

* Refactor deprecated code

* Add ompl repo (for test)

* Fix indent

* Remove OMPL

* Fix cppcheck errors (ros-planning#1718)

* Fix cppcheck errors

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix test name generator

* Fix returning after it is deallocated / released error

* Increase sleep to 10 seconds to allow map server node to start up

* Fix infinite wait for service in nav2_map_server tests

* [WIP] Checking line coverage in codecov (ros-planning#1713)

* Add parsers options

* Disable all branch detection option

* Replace lcov with gcov

* Revert changes in codecov.yaml and remove branch coverage from coverage bash script

* [master] Windows bring-up (ros-planning#1704)

* Windows bringup.

* nullity check.

* nullity check.

* Added goal updated condition node (ros-planning#1712)

* Added goal updated condition node

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Updated bt_navigator readme and added missing failure condition

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Updated bt_navigator readme

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Update nav2_bt_navigator/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_bt_navigator/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Update nav2_bt_navigator/README.md

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Tf timeout (ros-planning#1724)

* Parameterized tf_timeout in amcl
* Refactored the existing transform_tolerance parameter in amcl to transform_publish_shift.
* Refactored tf_buffer method calls to use transform_tolerance according to [978].

* Added tf_timeout to static_layer and observation_buffer

* declared transform_tolerance parameter

* change transform_tolerance default value

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Removed transform_publish_shift param

* Fixed linting errors

Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>

* Fix system_tests flaky issue (ros-planning#1723)

* Return back planning system tests

* Fix testcase failure related to not updating costmap

This appeared while compiler treated costmap pointer
to be unused and optimized it out

* Fix flake8 E128 failure

* Fix uninitialized variable warnings and change default value (ros-planning#1728)

* Fix uninitialized warning for variable temp_tf_tol.

* Change default GridBased.tolerance to 0.5 meters

* Add DistanceController decorator node (ros-planning#1699)

* Add DistanceController decorator node

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Update documentation and rename behavior tree XMLs

* Add tests for distance controller

* Fix test

* Update readme and add BT image

* Rename test files

* Remove protected setStatus calls

* Add option to inflate unknown space (ros-planning#1675)

* Add option to inflate around unknown space

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Fix bug regarding lower bound of double in worldToMapEnforceBounds

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Convert 2D caches to 1D vectors for automatic memory management and better locality

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add general optimizations and modern syntax

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Switch from map<double> to vector<> in using precached integer distances

Credits to original author from ros-planning/navigation#839

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add tests for inflate_unknown and inflate_around_unknown

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Remove commented out assert

Signed-off-by: Shrijit Singh <shrijitsingh99@gmail.com>

* Add BehaviorTree visualization support using Groot (ros-planning#1725)

* Add BehaviorTree visualization support using Groot

* Add nav2 TreeNodesModel containing all BT Nodes used

* Add instructions on using Groot to visualize behavior trees

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Rearrange files and minor updates

* Move nav2_tree_nodes.xml and groot instruction to nav2_beahvior_tree

* Run cmake install rules for nav2_tree_nodes.xml

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Add distance controller node in nav2 TreeNodesModel

Signed-off-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>

* Add collision checking for footprint without using subscibers. (ros-planning#1703)

* Add collision checking for footprint without using subscibers.

* Address reviewer's comments

- Changed the design if the footprint collision checkers
- And propogate the changes to dependencies such as nav2_recoveries and nav2_core

* Remove some extra headers

* Remove debuging code

* Add requested test

* Change weird test names.

* Remove unorientFootprint function dependency

* Imporve tests

* Fix commented Varible

* Expose distance controller frames to ports (ros-planning#1741)

* Add feedback in navigation2 actions (ros-planning#1736)

* adding navigate to pose feedback and remove random crawl from master

* adding controller feedback

* recovery feedback actions

* Update nav2_controller/src/nav2_controller.cpp

Co-Authored-By: Carl Delsey <1828778+cdelsey@users.noreply.github.com>

* Add feedback in wait action and make general improvements

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix rebase errors

* Make feedback reset across different goals

Co-authored-by: stevemacenski <stevenmacenski@gmail.com>
Co-authored-by: Carl Delsey <1828778+cdelsey@users.noreply.github.com>

* Switch to nightly tag for release image (ros-planning#1746)

as tests for connext rmw have been disabled.54

* Parameterization of the tf_tol argument for getCurrentPose calls (ros-planning#1735)

* parametrized collision_checker getCurrentPose timeout

* Parameterized tf_tol for spin and backup recoveries

* Parameterized tf_tol for goal_reached_condition

* moved transform_tolerance_ to recovery.hpp

* moved transform_tolerance parameter declaration to bt_navigator

* Fixed typo

* Fixed transform_tolerance_ location and transform_tolerance param declaration location

* Parameterized tf_tol for distance_controller.cpp

* Revert libgazebo11 workaround for CI (ros-planning#1750)

* Revert libgazebo11 workaround

* Revert gazebo_ros_pkgs to main ros2 branch

* Resolves ros-planning#938 (ros-planning#1747)

The forward_point_distance used in the GoalAlign and PathAlign critics projects the current pose forward a default 0.325 meters before scoring the trajectory. This can cause velocities that create sharp turns into obstacles to be chosen (reproducible with the Turtlebot3 simulation).

For TB3, which is small in size, 0.325 meters is too far. Instead, enable GoalAlign and PathAlign critics in the DWB controller and shorten the critics' forward_point_distance (how far the current pose is project given the current orientation before scoring the trajectory) from the default 0.325 meters to 0.1 meters which improves stability when running with TB3.

Signed-off-by: Jack Pien <jack@jackpien.com>

* make basic failing test for tf2 wrapper (start of robot utils tests) (ros-planning#1743)

* make basic failing test for tf2 wrapper

* add file :-)

* Run rosdep update in Dockerfile and CI (ros-planning#1751)

So we don't have to wait for the nightly parent image to update rosdep

* Don't pass rosdistro when using empty index (ros-planning#1752)

Context: osrf/docker_images#399

* Explicitly set junit_family parameter for nav2_gazebo_spawner tests (ros-planning#1753)

* Parameterize frame IDs (ros-planning#1742)

* Use parameterized frames (ros-planning#1726)

Signed-off-by: ymd-stella <world.applepie@gmail.com>

* Expore frame to ports in GoalReached and add params to bringu yaml files

* Fix recovery interface

* Update launch file and add recovery parameters to bringup yaml

* Remove lifecycle node params

* Update bringup param files

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix cpplint error

* Fix nav2_recoveries cpplint errors

Co-authored-by: ymd-stella <world.applepie@gmail.com>

* Add the goal to NavFN tolerance region traversing algorithm (ros-planning#1734)

* Consider the goal itself when looking to potential within tolerance region

* Fixed comments

* Re-enable nightly debug builds for codecov (ros-planning#1760)

Looks like codecov analytics from merged PRs alone do not update codecov status for the master branch.
Re-enabling nightly debug builds to keep codecov status on master up-to-date

* Wait for initial pose and increase timeouts (ros-planning#1759)

Initial pose is needed for the test to run
so it makes sense to wait for it till it times out.

* removed unused condition (ros-planning#1769)

* Added use of declare_parameter_if_not_declared. (ros-planning#1765)

* Added use of declare_parameter_if_not_declared.

* Fixed column width.

* Whitespace removal

* Style cleanup.

* a bugfix of clear costmap service action (ros-planning#1764)

The following is an example of the error.
[ERROR] [1590171638.335693813] [bt_navigator]: Action server failed while executing action callback: "failed to send request: ros_request argument is null, at /opt/ros/src/ros2/rcl/rcl/src/rcl/client.c:278"a bugfix of clear costmap service action

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Add condition nodes for time and distance replanning (ros-planning#1705)

* Add condition nodes and behavior tree to enable replan on new goal

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Fix time expired and distance traveled conditions

* Remove new_goal_received from blackboard

* Fix IDLE check condition in new condition nodes

* Fix lint errors

* Fix lint errors

* Address reviewer's comments

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Add tests

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* a patch for ros-planning#1773 BT navigator nodes with use_sim_time (ros-planning#1776)

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Replace deprecated launch params (ros-planning#1778)

* Update deprecated launch params

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* Revert internal python changes

Signed-off-by: Sarthak Mittal <sarthakmittal2608@gmail.com>

* adding backup tests (ros-planning#1774)

* adding backup tests

* add negative case

* correct termination logic

* increase speed

* fixish failure test case

* declared default_critic_namespaces param (ros-planning#1785)

* List of parameters in the stack  (ros-planning#1761)

* Initial commit

* Added AMCL params and some formatting fixes

* Added some missing params

* Added bt nodes ports

* fixed typo

* Added <> to base names + some reformating

* added default_critic_namespaces param to dwb_local_planner

* Fix some parameters not being passed to getCurrentPose (ros-planning#1790)

Signed-off-by: ymd-stella <world.applepie@gmail.com>

* Add SLAM Toolbox and map_saver_server into launch-files (ros-planning#1768)

* Add SLAM launch file

Fix possible collision of laser scan with camera on waffle

* Simplifications and fixes after review

* Increase save_map_timeout to 5 sec to comply with SLAM Toolbox map rate

* BT navigator conversion fix (ros-planning#1793)

* include bt_conversions.hpp to convert input properly

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* delete redundant include statement

Signed-off-by: Daisuke Sato <daisukes@cmu.edu>

* Add complete parameter description documentation (ros-planning#1786)

* adding a bunch of parameter descriptions

* ading costmap param descriptions

* add AMCL parameters

* adding DWB params

* resolving review questions

* include planner and controller ID parameters

* fixing jack's requests

* Test for lifecycle manager (ros-planning#1794)

* Add test

* Add more coverage

* Merge test header file with executable

* Address PR reviewer's comment

* Add some more coverage

* Revert accidental changes to localization_launch.py file.

* Update modified default_bt_xml_filename parameter (ros-planning#1797)

Parameter ``bt_xml_filename`` has changed to ``default_bt_xml_filename``

Co-authored-by: Aitor Miguel Blanco <aitormibl@gmail.com>
Co-authored-by: Shivang Patel <shivaang14@gmail.com>
Co-authored-by: Davide Faconti <davide.faconti@gmail.com>
Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
Co-authored-by: crthilakraj <crthilakraj@users.noreply.github.com>
Co-authored-by: chikmagalore.thilak <chikmagalore.thilak@bshg.com>
Co-authored-by: Ashwin Bose <ashwinbose123@gmail.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Shrijit Singh <shrijitsingh99@gmail.com>
Co-authored-by: p-vega <vega@enib.fr>
Co-authored-by: Ruffin <roxfoxpox@gmail.com>
Co-authored-by: Francisco Martín Rico <fmrico@gmail.com>
Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
Co-authored-by: Siddarth Gore <siddarth.gore@gmail.com>
Co-authored-by: Sarathkrishnan Ramesh <sarathkrishnan99@gmail.com>
Co-authored-by: Sean Yen <seanyen@microsoft.com>
Co-authored-by: Marwan Taher <marokhaled99@gmail.com>
Co-authored-by: TingChang <qting0529@hotmail.com>
Co-authored-by: Carl Delsey <1828778+cdelsey@users.noreply.github.com>
Co-authored-by: Jack Pien <jack@jackpien.com>
Co-authored-by: ymd-stella <world.applepie@gmail.com>
Co-authored-by: tgreier <tgreier@moog.com>
Co-authored-by: Daisuke Sato <43101027+daisukes@users.noreply.github.com>
Co-authored-by: ymd-stella <7959916+ymd-stella@users.noreply.github.com>
v-lopez added a commit to pal-robotics/pmb2_navigation that referenced this issue Mar 9, 2021
A similara issue seems to be reported in:
ros-planning/navigation2#938

Probably we need more tunning for our robot and/or a different
controller
@naidol
Copy link

naidol commented Dec 11, 2021

Hi all,
Not sure if you guys found a solution, but i did now in 2021. use this controller (link below) available in Ros2 Galactic or Foxy.
After much tweaking with the above DWB settings ... i tried this controller below ... it works perfect for my 4wd robot (rectangular shape - long wheel base).

https://index.ros.org/p/nav2_regulated_pure_pursuit_controller/#galactic

just cut and paste the to replace all the DWB controller nonsense above.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
2 - Medium Medium Priority help wanted Extra attention is needed
Projects
No open projects
V1.0.0
  
Done
Development

Successfully merging a pull request may close this issue.

7 participants