Skip to content

Commit

Permalink
Smoother error codes (ros-navigation#3296)
Browse files Browse the repository at this point in the history
* minimum error code set

* test for invalid smoother

* undo

* added rest of error tests

* Solve bug when CostmapInfoServer is reactivated (ros-navigation#3292)

* Solve bug when CostmapInfoServer is reactivated

* Smoothness metrics update (ros-navigation#3294)

* Update metrics for path smoothness

* Support Savitzky-Golay smoother

* preempt/cancel test for time behavior, spin pluguin (ros-navigation#3301)

* include preempt/cancel test for time behavior, spin pluguin

* linting

* fix bug in code

* removed changes to simple_smoother

* reverted simple_smoother

* revert

* revert

* updated constrained smoother

* revert

* added smoother error for invalid path

* linting

* invalid path test

* added error codes

* Timeout exception thrown by smoothers

* code review

Co-authored-by: Joshua Wallace <josho.wallace.com>
Co-authored-by: MartiBolet <43337758+MartiBolet@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Stevedan Ogochukwu Omodolor <61468301+stevedanomodolor@users.noreply.github.com>
  • Loading branch information
4 people authored and Andrew Lycas committed Feb 23, 2023
1 parent bb21242 commit e40c825
Show file tree
Hide file tree
Showing 21 changed files with 429 additions and 42 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,10 @@ namespace nav2_behavior_tree
*/
class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::action::SmoothPath>
{
using Action = nav2_msgs::action::SmoothPath;
using ActionResult = Action::Result;
using ActionGoal = Action::Goal;

public:
/**
* @brief A constructor for nav2_behavior_tree::SmoothPathAction
Expand All @@ -52,6 +56,16 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
*/
BT::NodeStatus on_success() override;

/**
* @brief Function to perform some user-defined operation upon abortion of the action
*/
BT::NodeStatus on_aborted() override;

/**
* @brief Function to perform some user-defined operation upon cancellation of the action
*/
BT::NodeStatus on_cancelled() override;

/**
* @brief Creates list of BT ports
* @return BT::PortsList Containing basic ports along with node-specific ports
Expand All @@ -60,18 +74,20 @@ class SmoothPathAction : public nav2_behavior_tree::BtActionNode<nav2_msgs::acti
{
return providedBasicPorts(
{
BT::OutputPort<nav_msgs::msg::Path>(
"smoothed_path",
"Path smoothed by SmootherServer node"),
BT::OutputPort<double>("smoothing_duration", "Time taken to smooth path"),
BT::OutputPort<bool>(
"was_completed", "True if smoothing was not interrupted by time limit"),
BT::InputPort<nav_msgs::msg::Path>("unsmoothed_path", "Path to be smoothed"),
BT::InputPort<double>("max_smoothing_duration", 3.0, "Maximum smoothing duration"),
BT::InputPort<bool>(
"check_for_collisions", false,
"If true collision check will be performed after smoothing"),
BT::InputPort<std::string>("smoother_id", ""),
BT::OutputPort<nav_msgs::msg::Path>(
"smoothed_path",
"Path smoothed by SmootherServer node"),
BT::OutputPort<double>("smoothing_duration", "Time taken to smooth path"),
BT::OutputPort<bool>(
"was_completed", "True if smoothing was not interrupted by time limit"),
BT::OutputPort<ActionResult::_error_code_type>(
"error_code_id", "The smooth path error code"),
});
}
};
Expand Down
15 changes: 15 additions & 0 deletions nav2_behavior_tree/plugins/action/smooth_path_action.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,21 @@ BT::NodeStatus SmoothPathAction::on_success()
setOutput("smoothed_path", result_.result->path);
setOutput("smoothing_duration", rclcpp::Duration(result_.result->smoothing_duration).seconds());
setOutput("was_completed", result_.result->was_completed);
// Set empty error code, action was successful
setOutput("error_code_id", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}

BT::NodeStatus SmoothPathAction::on_aborted()
{
setOutput("error_code_id", result_.result->error_code);
return BT::NodeStatus::FAILURE;
}

BT::NodeStatus SmoothPathAction::on_cancelled()
{
// Set empty error code, action was cancelled
setOutput("error_code_id", ActionGoal::NONE);
return BT::NodeStatus::SUCCESS;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@

#include "nav2_constrained_smoother/smoother_cost_function.hpp"
#include "nav2_constrained_smoother/utils.hpp"
#include "nav2_core/smoother_exceptions.hpp"

#include "ceres/ceres.h"
#include "Eigen/Core"
Expand Down Expand Up @@ -94,7 +95,7 @@ class Smoother
{
// Path has always at least 2 points
if (path.size() < 2) {
throw std::runtime_error("Constrained smoother: Path must have at least 2 points");
throw nav2_core::InvalidPath("Constrained smoother: Path must have at least 2 points");
}

options_.max_solver_time_in_seconds = params.max_time;
Expand All @@ -110,7 +111,7 @@ class Smoother
RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "%s", summary.FullReport().c_str());
}
if (!summary.IsSolutionUsable() || summary.initial_cost - summary.final_cost < 0.0) {
return false;
throw nav2_core::FailedToSmoothPath("Solution is not usable");
}
} else {
RCLCPP_INFO(rclcpp::get_logger("smoother_server"), "Path too short to optimize");
Expand Down
6 changes: 3 additions & 3 deletions nav2_constrained_smoother/src/constrained_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@
#include "nav2_constrained_smoother/constrained_smoother.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "nav2_core/smoother_exceptions.hpp"

#include "pluginlib/class_loader.hpp"
#include "pluginlib/class_list_macros.hpp"
Expand Down Expand Up @@ -137,8 +137,8 @@ bool ConstrainedSmoother::smooth(nav_msgs::msg::Path & path, const rclcpp::Durat
logger_,
"%s: failed to smooth plan, Ceres could not find a usable solution to optimize.",
plugin_name_.c_str());
throw std::runtime_error(
"Failed to smooth plan, Ceres could not find a usable solution.");
throw nav2_core::FailedToSmoothPath(
"Failed to smooth plan, Ceres could not find a usable solution");
}

// populate final path
Expand Down
67 changes: 67 additions & 0 deletions nav2_core/include/nav2_core/smoother_exceptions.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,67 @@
// Copyright (c) 2022 Joshua Wallace
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_
#define NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_

#include <stdexcept>
#include <string>

namespace nav2_core
{

class SmootherException : public std::runtime_error
{
public:
explicit SmootherException(const std::string & description)
: std::runtime_error(description) {}
};

class InvalidSmoother : public SmootherException
{
public:
explicit InvalidSmoother(const std::string & description)
: SmootherException(description) {}
};

class InvalidPath : public SmootherException
{
public:
explicit InvalidPath(const std::string & description)
: SmootherException(description) {}
};

class SmootherTimedOut : public SmootherException
{
public:
explicit SmootherTimedOut(const std::string & description)
: SmootherException(description) {}
};

class SmoothedPathInCollision : public SmootherException
{
public:
explicit SmoothedPathInCollision(const std::string & description)
: SmootherException(description) {}
};

class FailedToSmoothPath : public SmootherException
{
public:
explicit FailedToSmoothPath(const std::string & description)
: SmootherException(description) {}
};

} // namespace nav2_core
#endif // NAV2_CORE__SMOOTHER_EXCEPTIONS_HPP_
11 changes: 11 additions & 0 deletions nav2_msgs/action/SmoothPath.action
Original file line number Diff line number Diff line change
@@ -1,3 +1,13 @@
# Error codes
# Note: The expected priority order of the errors should match the message order
uint16 NONE=0
uint16 UNKNOWN=500
uint16 INVALID_SMOOTHER=501
uint16 TIMEOUT=502
uint16 SMOOTHED_PATH_IN_COLLISION=503
uint16 FAILED_TO_SMOOTH_PATH=504
uint16 INVALID_PATH=505

#goal definition
nav_msgs/Path path
string smoother_id
Expand All @@ -8,5 +18,6 @@ bool check_for_collisions
nav_msgs/Path path
builtin_interfaces/Duration smoothing_duration
bool was_completed
uint16 error_code
---
#feedback definition
10 changes: 6 additions & 4 deletions nav2_simple_commander/nav2_simple_commander/robot_navigator.py
Original file line number Diff line number Diff line change
Expand Up @@ -433,16 +433,18 @@ def _smoothPathImpl(self, path, smoother_id='', max_duration=2.0, check_for_coll
self.result_future = self.goal_handle.get_result_async()
rclpy.spin_until_future_complete(self, self.result_future)
self.status = self.result_future.result().status
if self.status != GoalStatus.STATUS_SUCCEEDED:
self.warn(f'Getting path failed with status code: {self.status}')
return None

return self.result_future.result().result

def smoothPath(self, path, smoother_id='', max_duration=2.0, check_for_collision=False):
"""Send a `SmoothPath` action request."""
rtn = self._smoothPathImpl(
self, path, smoother_id='', max_duration=2.0, check_for_collision=False)
path, smoother_id='', max_duration=2.0, check_for_collision=False)

if self.status != GoalStatus.STATUS_SUCCEEDED:
self.warn(f'Getting path failed with status code: {self.status}')
return None

if not rtn:
return None
else:
Expand Down
8 changes: 8 additions & 0 deletions nav2_smoother/include/nav2_smoother/nav2_smoother.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,7 @@ class SmootherServer : public nav2_util::LifecycleNode
nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;

using Action = nav2_msgs::action::SmoothPath;
using ActionGoal = Action::Goal;
using ActionServer = nav2_util::SimpleActionServer<Action>;

/**
Expand All @@ -135,6 +136,13 @@ class SmootherServer : public nav2_util::LifecycleNode
*/
bool findSmootherId(const std::string & c_name, std::string & name);

/**
* @brief Validate that the path contains a meaningful path for smoothing
* @param path current path
* return bool if the path is valid
*/
bool validate(const nav_msgs::msg::Path & path);

// Our action server implements the SmoothPath action
std::unique_ptr<ActionServer> action_server_;

Expand Down
61 changes: 53 additions & 8 deletions nav2_smoother/src/nav2_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,7 @@
#include <utility>
#include <vector>

#include "nav2_core/planner_exceptions.hpp"
#include "nav2_core/smoother_exceptions.hpp"
#include "nav2_smoother/nav2_smoother.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav_2d_utils/conversions.hpp"
Expand Down Expand Up @@ -263,13 +263,17 @@ void SmootherServer::smoothPlan()
if (findSmootherId(c_name, current_smoother)) {
current_smoother_ = current_smoother;
} else {
action_server_->terminate_current();
return;
throw nav2_core::InvalidSmoother("Invalid Smoother: " + c_name);
}

// Perform smoothing
auto goal = action_server_->get_current_goal();
result->path = goal->path;

if (!validate(result->path)) {
throw nav2_core::InvalidPath("Requested path to smooth is invalid");
}

result->was_completed = smoothers_[current_smoother_]->smooth(
result->path, goal->max_smoothing_duration);
result->smoothing_duration = steady_clock_.now() - start_time;
Expand All @@ -283,6 +287,7 @@ void SmootherServer::smoothPlan()
rclcpp::Duration(goal->max_smoothing_duration).seconds(),
rclcpp::Duration(result->smoothing_duration).seconds());
}

plan_publisher_->publish(result->path);

// Check for collisions
Expand All @@ -299,8 +304,11 @@ void SmootherServer::smoothPlan()
get_logger(),
"Smoothed path leads to a collision at x: %lf, y: %lf, theta: %lf",
pose2d.x, pose2d.y, pose2d.theta);
action_server_->terminate_current(result);
return;
throw nav2_core::SmoothedPathInCollision(
"Smoothed Path collided at"
"X: " + std::to_string(pose2d.x) +
"Y: " + std::to_string(pose2d.y) +
"Theta: " + std::to_string(pose2d.theta));
}
fetch_data = false;
}
Expand All @@ -311,13 +319,50 @@ void SmootherServer::smoothPlan()
rclcpp::Duration(result->smoothing_duration).seconds());

action_server_->succeeded_current(result);
} catch (nav2_core::PlannerException & e) {
RCLCPP_ERROR(this->get_logger(), e.what());
action_server_->terminate_current();
} catch (nav2_core::InvalidSmoother & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
result->error_code = ActionGoal::INVALID_SMOOTHER;
action_server_->terminate_current(result);
return;
} catch (nav2_core::SmootherTimedOut & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
result->error_code = ActionGoal::TIMEOUT;
action_server_->terminate_current(result);
return;
} catch (nav2_core::SmoothedPathInCollision & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
result->error_code = ActionGoal::SMOOTHED_PATH_IN_COLLISION;
action_server_->terminate_current(result);
return;
} catch (nav2_core::FailedToSmoothPath & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
result->error_code = ActionGoal::FAILED_TO_SMOOTH_PATH;
action_server_->terminate_current(result);
return;
} catch (nav2_core::InvalidPath & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
result->error_code = ActionGoal::INVALID_PATH;
action_server_->terminate_current(result);
return;
} catch (nav2_core::SmootherException & ex) {
RCLCPP_ERROR(this->get_logger(), "%s", ex.what());
result->error_code = ActionGoal::UNKNOWN;
action_server_->terminate_current(result);
return;
}
}

bool SmootherServer::validate(const nav_msgs::msg::Path & path)
{
if (path.poses.empty()) {
RCLCPP_WARN(get_logger(), "Requested path to smooth is empty");
return false;
}

RCLCPP_DEBUG(get_logger(), "Requested path to smooth is valid");
return true;
}

} // namespace nav2_smoother

#include "rclcpp_components/register_node_macro.hpp"
Expand Down
3 changes: 2 additions & 1 deletion nav2_smoother/src/savitzky_golay_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
#include <vector>
#include <memory>
#include "nav2_smoother/savitzky_golay_smoother.hpp"
#include "nav2_core/smoother_exceptions.hpp"

namespace nav2_smoother
{
Expand Down Expand Up @@ -71,7 +72,7 @@ bool SavitzkyGolaySmoother::smooth(
RCLCPP_WARN(
logger_,
"Smoothing time exceeded allowed duration of %0.2f.", max_time.seconds());
return false;
throw nav2_core::SmootherTimedOut("Smoothing time exceed allowed duration");
}

// Smooth path segment
Expand Down
Loading

0 comments on commit e40c825

Please sign in to comment.