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

Fix zero waypoints crash #1978

Merged
merged 3 commits into from
Sep 3, 2020
Merged

Fix zero waypoints crash #1978

merged 3 commits into from
Sep 3, 2020

Conversation

wilcobonestroo
Copy link
Contributor


Basic Info

Info Please fill out this column
Ticket(s) this addresses #1977
Primary OS tested on Ubuntu 20.04
Robotic platform tested on Gazebo simulation of turtlebot 3

Description of contribution in a few bullet points

Check for the number of waypoints. If there are no waypoints, then terminate the action and return.

Description of documentation updates required from your changes

No updates to documentation required.

@@ -127,6 +127,11 @@ WaypointFollower::followWaypoints()
get_logger(), "Received follow waypoint request with %i waypoints.",
static_cast<int>(goal->poses.size()));

if (goal->poses.size() == 0) {
action_server_->terminate_all();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this should auto-succeed rather than terminate since it technically succeeded in doing nothing

@wilcobonestroo
Copy link
Contributor Author

I was looking for more information/documentation about how the Action Server should respond. I encountered the Fibonacci examples. However, I did not find detailed explanations (including cpp examples). Do you know where I can find such info?

@SteveMacenski
Copy link
Member

can you be more specific? I don't know what you mean by "how it should respond"

@wilcobonestroo
Copy link
Contributor Author

I was not sure what an action server should do on success. For example, In the explanation of actions there is a description of the states and the state transitions. So, I was looking for the succeed state transition (function) in the action server. However, it turned out to be succeeded_current(result). I assumed that the result, in this case the list of missed waypoints, should be empty. However, I am guessing and I am not sure if this is the right way. I was looking for material that would describe how the action server should respond.

@SteveMacenski SteveMacenski merged commit a359532 into ros-navigation:main Sep 3, 2020
@SteveMacenski
Copy link
Member

SteveMacenski commented Sep 3, 2020

Yeah that was the correct way to handle it. Terminate is used when it failed (and terminate all is used to terminate not just the current action, but all pending actions as well). We use succeeded_current for a successful outcome to send back the correct result code.

Can't have failed waypoints when there are no waypoints.

@wilcobonestroo
Copy link
Contributor Author

wilcobonestroo commented Sep 3, 2020

Check. The descriptions in the code/API and the code of the SimpleActionServer are also useful for me.

@wilcobonestroo wilcobonestroo deleted the fix-zero-waypoints-crash branch September 3, 2020 19:25
SteveMacenski pushed a commit that referenced this pull request Nov 4, 2020
* return if the number of waypoints is zero.

* terminate the action.

* Succeed action instead of terminating.
SteveMacenski added a commit that referenced this pull request Nov 4, 2020
* initialize variables in inflation layer (#1970)

* Fix zero waypoints crash (#1978)

* return if the number of waypoints is zero.

* terminate the action.

* Succeed action instead of terminating.

* Add IsBatteryLow condition node (#1974)

* Add IsBatteryLow condition node

* Update default battery topic and switch to battery %

* Fix test

* Switch to sensor_msgs/BatteryState

* Add option to use voltage by default or switch to percentage

* Add sensor_msgs dependency in package.xml

* Make percentage default over voltage

* Update parameter list

* Initialize inflate_cone_ variable. (#1988)

* Initialize inflate_cone_ variable.

* initialize inflate_cone_ based on parameter.

* Increase the sleep time in the tests makes the costmap test always succeed on my machine.

* Add timeouts to all spin_until_future_complete calls (#1998)

* Add timeouts to all spin_until_future_complete calls

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

* Update default timeout value

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

* Controllers should not be influenced by time jumps or slew (#2012)

* Controllers should not be influenced by time jumps

Therefore use rclcpp::GenericRate<std::chrono::steady_clock> instead of
rclcpp::Rate

Signed-off-by: Martijn Buijs <martijn.buijs@gmail.com>

* Change to using `rclcpp::WallRate` for better readability

Signed-off-by: Martijn Buijs <martijn.buijs@gmail.com>

* Fix max path cycles for case where map has larger Y dimension than X dimension (#2017)

* Fix max path cycles for case where map has larger Y dimension than X dimension

* Improve readability

* fix ament_cpplint and ament_uncrustify issues

* fix minor cherry pick conflict mistake

* bump version to 0.4.4

Co-authored-by: Michael Ferguson <mfergs7@gmail.com>
Co-authored-by: Wilco Bonestroo <w.j.bonestroo@saxion.nl>
Co-authored-by: Sarthak Mittal <sarthakmittal2608@gmail.com>
Co-authored-by: Martijn Buijs <Martijn.buijs@gmail.com>
Co-authored-by: justinIRBT <69175069+justinIRBT@users.noreply.github.com>
ruffsl pushed a commit to ruffsl/navigation2 that referenced this pull request Jul 2, 2021
* return if the number of waypoints is zero.

* terminate the action.

* Succeed action instead of terminating.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants