Add acceleration limits to DriveOnHeading and BackUp behaviors #4810
Conversation
|
@RBT22, your PR has failed to build. Please check CI outputs and resolve issues. |
Signed-off-by: RBT22 <rozgonyibalint@gmail.com>
Signed-off-by: RBT22 <rozgonyibalint@gmail.com>
Signed-off-by: RBT22 <rozgonyibalint@gmail.com>
Signed-off-by: RBT22 <rozgonyibalint@gmail.com>
|
@RBT22, your PR has failed to build. Please check CI outputs and resolve issues. |
RBT22
force-pushed
the
rolling/acc-drive-on-heading
branch
from
a6633a8 to
228de3b
Compare
|
@RBT22, your PR has failed to build. Please check CI outputs and resolve issues. |
| @@ -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); | |||
There was a problem hiding this comment.
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
| if (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0) { | ||
| cmd_vel->twist.linear.x = command_speed_; | ||
| } else { | ||
| double current_speed = last_vel_ ? *last_vel_ : 0.0; |
There was a problem hiding this comment.
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
| @@ -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) { | |||
There was a problem hiding this comment.
||? 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 = 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; |
There was a problem hiding this comment.
| bool forward = command_speed_ > 0.0 ? true : false; | |
| bool forward = command_speed_ > 0.0; |
😉
Basic Info
Description of contribution in a few bullet points
Added two params:
acceleration_limitanddeceleration_limitfor DriveOnHeading and BackUp behaviors.Description of documentation updates required from your changes
Added new parameter, so need to add that to default configs and documentation page
Future work that may be required in bullet points
For Maintainers: