Skip to content

Commit

Permalink
change param name
Browse files Browse the repository at this point in the history
  • Loading branch information
Guillaume Doisy committed Mar 31, 2021
1 parent 2eb7288 commit 9a65af8
Show file tree
Hide file tree
Showing 2 changed files with 6 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -229,7 +229,7 @@ class ControllerServer : public nav2_util::LifecycleNode
double min_y_velocity_threshold_;
double min_theta_velocity_threshold_;

double controller_patience_;
double failure_tolerance_;

// Whether we've published the single controller warning yet
geometry_msgs::msg::Pose end_pose_;
Expand Down
10 changes: 5 additions & 5 deletions nav2_controller/src/nav2_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ ControllerServer::ControllerServer()

declare_parameter("speed_limit_topic", rclcpp::ParameterValue("speed_limit"));

declare_parameter("controller_patience", rclcpp::ParameterValue(0.0));
declare_parameter("failure_tolerance", rclcpp::ParameterValue(0.0));

// The costmap node is used in the implementation of the controller
costmap_ros_ = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
Expand Down Expand Up @@ -111,7 +111,7 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & state)

std::string speed_limit_topic;
get_parameter("speed_limit_topic", speed_limit_topic);
get_parameter("controller_patience", controller_patience_);
get_parameter("failure_tolerance", failure_tolerance_);

costmap_ros_->on_configure(state);

Expand Down Expand Up @@ -399,7 +399,7 @@ void ControllerServer::computeAndPublishVelocity()
goal_checker_.get());
last_valid_cmd_time_ = now();
} catch (nav2_core::PlannerException & e) {
if (controller_patience_ > 0 || controller_patience_ == -1.0) {
if (failure_tolerance_ > 0 || failure_tolerance_ == -1.0) {
RCLCPP_WARN(this->get_logger(), e.what());
cmd_vel_2d.twist.angular.x = 0;
cmd_vel_2d.twist.angular.y = 0;
Expand All @@ -409,8 +409,8 @@ void ControllerServer::computeAndPublishVelocity()
cmd_vel_2d.twist.linear.z = 0;
cmd_vel_2d.header.frame_id = costmap_ros_->getBaseFrameID();
cmd_vel_2d.header.stamp = now();
if ((now() - last_valid_cmd_time_).seconds() > controller_patience_ &&
controller_patience_ != -1.0)
if ((now() - last_valid_cmd_time_).seconds() > failure_tolerance_ &&
failure_tolerance_ != -1.0)
{
throw nav2_core::PlannerException("Controller patience exceeded");
}
Expand Down

0 comments on commit 9a65af8

Please sign in to comment.