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.

Sign up for GitHub

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

Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

Add acceleration limits to DriveOnHeading and BackUp behaviors #4810

Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2804b7e
Add acceleration constraints
RBT22 Dec 20, 2024
adbdfe1
Cleanup code
RBT22 Dec 20, 2024
02f18ce
Format code
RBT22 Dec 20, 2024
228de3b
Add <limits> header to drive_on_heading.hpp
RBT22 Dec 20, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
49 changes: 48 additions & 1 deletion nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <chrono>
#include <memory>
#include <utility>
#include <limits>

#include "nav2_behaviors/timed_behavior.hpp"
#include "nav2_msgs/action/drive_on_heading.hpp"
Expand Down Expand Up @@ -133,7 +134,35 @@ class DriveOnHeading : public TimedBehavior<ActionT>
cmd_vel->header.frame_id = this->robot_base_frame_;
cmd_vel->twist.linear.y = 0.0;
cmd_vel->twist.angular.z = 0.0;
cmd_vel->twist.linear.x = command_speed_;
if (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0) {
Copy link
Member

@SteveMacenski SteveMacenski Dec 20, 2024 •
edited
Loading

Choose a reason for hiding this comment

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

||? I assume we have to set both if we want to use either. That would let you remove the 2x if in the other branch as well. ALso we can simply if the forward && to just forward and !forward

Specifying one but not the other seems like an unsupportable configuration

cmd_vel->twist.linear.x = command_speed_;
} else {
double current_speed = last_vel_ ? *last_vel_ : 0.0;
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 the behaviors have a odom subscriber to use for the current velocity.

This actually brings up a whole topic about whether we should base the velocity closed loop (on odom speed) or open looop (on the last command sent, assuming it was executed). We have this also topic in the controllers and velocity smoother. If we're adding in dynamic constraints here, I think its sensible we decide what makes sense here as well

auto remaining_distance = std::fabs(command_x_) - distance;
double min_feasible_speed = -std::numeric_limits<double>::infinity();
double max_feasible_speed = std::numeric_limits<double>::infinity();
if (deceleration_limit_ > 0.0) {
min_feasible_speed = current_speed - deceleration_limit_ / this->cycle_frequency_;
}
if (acceleration_limit_ > 0.0) {
max_feasible_speed = current_speed + acceleration_limit_ / this->cycle_frequency_;
}
cmd_vel->twist.linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed);

// Check if we need to slow down to avoid overshooting
bool forward = command_speed_ > 0.0 ? true : false;
Copy link
Member

Choose a reason for hiding this comment

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

Suggested change
bool forward = command_speed_ > 0.0 ? true : false;
bool forward = command_speed_ > 0.0;

😉

if (forward && deceleration_limit_ > 0.0) {
double max_vel_to_stop = std::sqrt(2.0 * deceleration_limit_ * remaining_distance);
if (max_vel_to_stop < cmd_vel->twist.linear.x) {
cmd_vel->twist.linear.x = max_vel_to_stop;
}
} else if (!forward && acceleration_limit_ > 0.0) {
double max_vel_to_stop = -std::sqrt(2.0 * acceleration_limit_ * remaining_distance);
if (max_vel_to_stop > cmd_vel->twist.linear.x) {
cmd_vel->twist.linear.x = max_vel_to_stop;
}
}
}

geometry_msgs::msg::Pose2D pose2d;
pose2d.x = current_pose.pose.position.x;
Expand All @@ -146,6 +175,8 @@ class DriveOnHeading : public TimedBehavior<ActionT>
return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
}

last_vel_ = std::make_shared<double>(cmd_vel->twist.linear.x);
Copy link
Member

Choose a reason for hiding this comment

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

This is weird. If you want to initialize it so you know its valid, you could default it to numerical_limits<double>::max() and reset to that / check for it if we have a last value. I don't like this smart pointer stuff though


this->vel_pub_->publish(std::move(cmd_vel));

return ResultStatus{Status::RUNNING, ActionT::Result::NONE};
Expand All @@ -157,6 +188,10 @@ class DriveOnHeading : public TimedBehavior<ActionT>
*/
CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}

void onCleanup() override {last_vel_.reset();}

void onActionCompletion() override {last_vel_.reset();}

protected:
/**
* @brief Check if pose is collision free
Expand Down Expand Up @@ -214,6 +249,15 @@ class DriveOnHeading : public TimedBehavior<ActionT>
node,
"simulate_ahead_time", rclcpp::ParameterValue(2.0));
node->get_parameter("simulate_ahead_time", simulate_ahead_time_);

nav2_util::declare_parameter_if_not_declared(
node, this->behavior_name_ + ".acceleration_limit",
rclcpp::ParameterValue(0.0));
nav2_util::declare_parameter_if_not_declared(
node, this->behavior_name_ + ".deceleration_limit",
rclcpp::ParameterValue(0.0));
node->get_parameter(this->behavior_name_ + ".acceleration_limit", acceleration_limit_);
node->get_parameter(this->behavior_name_ + ".deceleration_limit", deceleration_limit_);
}

typename ActionT::Feedback::SharedPtr feedback_;
Expand All @@ -225,6 +269,9 @@ class DriveOnHeading : public TimedBehavior<ActionT>
rclcpp::Duration command_time_allowance_{0, 0};
rclcpp::Time end_time_;
double simulate_ahead_time_;
double acceleration_limit_;
double deceleration_limit_;
std::shared_ptr<double> last_vel_;
};

} // namespace nav2_behaviors
Expand Down
Loading