-
Notifications
You must be signed in to change notification settings - Fork 1.3k
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
Backward docking without sensors #4749
base: main
Are you sure you want to change the base?
Backward docking without sensors #4749
Conversation
c915ebd
to
14faf82
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think we can do this a bit better - but I appreciate the first prototype and your contributions!
nav2_docking/opennav_docking/include/opennav_docking/controller.hpp
Outdated
Show resolved
Hide resolved
@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues. |
ad0982d
to
4345fc5
Compare
@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues. |
80ba35f
to
e1a6598
Compare
@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues. |
d357701
to
d292774
Compare
@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues. |
96b2730
to
d2a10df
Compare
@Jakubach, your PR has failed to build. Please check CI outputs and resolve issues. |
dca434d
to
0376cf5
Compare
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Did you test this on your robot platforms as well to validate this works?
I also see some new lines or removal of previous empty lines, try to revert those changes please :-)
@@ -63,6 +64,7 @@ TEST(ControllerTests, DynamicParameters) { | |||
rclcpp::Parameter("controller.v_linear_min", 5.0), | |||
rclcpp::Parameter("controller.v_linear_max", 6.0), | |||
rclcpp::Parameter("controller.v_angular_max", 7.0), | |||
rclcpp::Parameter("controller.v_angular_min", 2.0), |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
- dock backwards + dock blind checks
{ | ||
std::lock_guard<std::mutex> lock(dynamic_params_lock_); | ||
cmd = control_law_->calculateRegularVelocity(pose, backward); | ||
cmd = control_law_->calculateRegularVelocity(pose, robot_pose, backward); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Is this related to the other item on reverse issues? I'd like @ajtudela's thoughts on this but I don't have a problem with it. I'm just surprised none of us thought about this if it was that simple. It removes the other work in the other file with the movement of target points
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
In my case (humble, Ubuntu 22.04) that was a part of fixing docking problem posted previously.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'll try to explain them in detail here:
- calculateRegularVelocity(target_pose, current_pose, backward) is used to calculate the velocity from the current_pose.
- calculateRegularVelocity(target_pose, backward) is used to calculate the velocity from the origin (or base frame of the robot). This function internally calls calculateRegularVelocity(target_pose, {0,0,0}, backward). If current_pose is (0,0,0), the result function will be the same.
Internally, the velocitywould be calculated using ~ the difference between the two poses.
So if your target_pose is different from the robot_frame, you could do the following
robot_pose = getRobotPoseInFrame(target_frame)
calculateRegularVelocity(target_pose, robot_pose, backward)
OR
transform(target_pose, base_frame_)
calculateRegularVelocity(target_pose, backward)
The speed would be the same.
Because @Jakubach has removed the backward projection and the later transformation, they should use the 'full' calculateRegularVelocity instead of the 'simpler' one.
I hope my explanation is clear enough.
// If you have backward_blind and dock_backward then | ||
// we know we need to do the initial rotation to go from forward to reverse | ||
// before doing the rest of the procedure. The initial_rotation would thus always be true. | ||
initial_rotation_ = true; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Does initial_rotation_
even need to be a parameter? If backward_blind_ = true
don't we always do the rotation?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
It also seems unused in the code itself :-)
} | ||
if (name == "backward_blind") { | ||
backward_blind_ = parameter.as_bool(); | ||
initial_rotation_ = parameter.as_bool(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You shouldn't update other parameters -- remove dock backward / initial rotation setting
vel.linear.x = 0.0; | ||
vel.angular.z = 0.0; | ||
if(angle_to_target > 0) { | ||
vel.angular.z = std::clamp(1.0 * angle_to_target * v_angular_max_, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
1.0 *
in both expressions seems unnecessary. I think the other one might mean -1.0
.
What is the derivation of this rotation code for angle_to_target * v_angular_max
? Might it be better to steal the rotation code from something like the rotation shim controller / spin behavior to make this consistent?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Odometric error is often large with high acceleration, so it would probably be wise to have acceleration / decelleration in the rotation action such that we minimize slippage -- since especially in this case, we're fully dead reckoning
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
@SteveMacenski i am almost done with that, but how should I take the current angular velocity inside docking_server in the good way?
shim controller code requires it and I am trying to take that
RotationShimController::computeRotateToHeadingCommand( |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You could get it the same way we do in the controller server with an odometry subscriber:
nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist()); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thank you @SteveMacenski, it's already implemented and I hope I managed it well.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I just noticed that from time to time (not in 100% of situations) robot slows down during performing an initial rotation. The angular distance is correct but robot the generatated velocities are like 0.003 rad/s
Now I am inspecting why that happens. I am testing with rotate_to_heading_angular_vel: 0.5
and rotate_to_heading_max_angular_accel: 0.5
EDIT: It's not related with that code. The code is fine but I have a strange problem with velocities during rotation (angular velocity goes to 0 in half of the turn, then the controller couldn't go out from this situation, but it's something from smoother or odometry maybe).
double angle_to_goal; | ||
auto command = std::make_unique<geometry_msgs::msg::TwistStamped>(); | ||
while(rclcpp::ok()){ | ||
angle_to_goal = angles::shortest_angular_distance(tf2::getYaw(robot_pose.pose.orientation), |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A question to ask: would we rather (1) rotate purely 180 from the staging pose and have the control law handle any deviation in the staging pose or (2) rotate approximately 180, taking into account any angular offsets present in the staging pose so that its as straight-backward as possible?
If (1), then we can just take the robot orientation and the target is just PI from that, no need to look at the dock pose.
If (2), then I think we need to look at the dock pose's orientation, not its relative position angle. But, I would agree this is up for debate about what's better. Another area @ajtudela's opinion would be appreciated :-)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Same here, I'd say (2) and just go straight to the dock but I'll have it tested the two versions in the lab tomorrow with our robots.
angle_to_goal = angles::shortest_angular_distance(tf2::getYaw(robot_pose.pose.orientation), | ||
atan2(robot_pose.pose.position.y - dock_pose.pose.position.y, | ||
robot_pose.pose.position.x - dock_pose.pose.position.x)); | ||
if(fabs(angle_to_goal) > 0.1){ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This should either be (1) a configurable threshold or (2) wait until its exceeded so its at least that much of a turn
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I made a mistake here, there should be an opposite sign (<). I will reimplement with 1 or 2. Thanks
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I called it tolerance in a hurry, but tell me please if it better should be "threshold"
// Thus, we backward project the controller's target pose a little bit after the | ||
// dock so that the robot never gets to the end of the spiral before its in contact | ||
// with the dock to stop the docking procedure. | ||
const double backward_projection = 0.25; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Wait a moment... even if we have the reversing signs handled another way, this is strictly necessary so that at the end of the trajectory near the dock, we continue to have a smooth motion rather than hitting the end of the trajectory singularity! We need this strictly speaking even for forward docking as the comment above mentions
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Handled in and reverted that code.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I believe your PR outright removes this bit, not adjust it. I'm on board with the adjustment if @ajtudela is
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I see the change in your recent pushes. Can we make (1 - 2 * dock_backwards_) *
a little more clear? I don't like to use boolean properties in math expressions in this way, its a bit obtuse to parse.
You could just use an if/else for setting a negative sign
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I share your opinion
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You need to flip the orientation before you backward project it, no? That way we always apply the point behind the docked pose, instead of 0.25m in front of the docked pose. ... or was the order incorrect beforehand (but I'm thinking not)
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I had similar observation. Putting that code before conditional statement was a fix but I was not sure about that. Now it's confirmed I think.
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Huh, that is interesting that everyone doing backward docking missed this -- sounds good
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Huh, that is interesting that everyone doing backward docking missed this -- sounds good
Yeah, the only thing I can think of is that they set the dock_pose much farther than the real dock, so the robot will bump the dock but never reach the target pose.
It would be nice to ask someone with such a robot what their settings are.
2f116b7
to
034b258
Compare
Codecov ReportAttention: Patch coverage is
|
2b47cb8
to
3ab0010
Compare
Let me know when you want me to review again - I see that you're pushing frequent updates |
// The control law can get jittery when close to the end when atan2's can explode. | ||
// Thus, we backward project the controller's target pose a little bit after the | ||
// dock so that the robot never gets to the end of the spiral before its in contact | ||
// with the dock to stop the docking procedure. | ||
const double backward_projection = 0.25; | ||
const double yaw = tf2::getYaw(target_pose.pose.orientation); | ||
target_pose.pose.position.x += cos(yaw) * backward_projection; | ||
target_pose.pose.position.y += sin(yaw) * backward_projection; | ||
tf2_buffer_->transform(target_pose, target_pose, base_frame_); | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You need to project the motion target a little further. Otherwise the control law will be very unstable at very short distances from the target.
In the graceful controller, there are similar tricks to avoid this unstability.
@@ -557,7 +571,7 @@ bool DockingServer::getCommandToPose( | |||
tf2_buffer_->transform(target_pose, target_pose, base_frame_); | |||
|
|||
// Compute velocity command | |||
if (!controller_->computeVelocityCommand(target_pose.pose, cmd, backward)) { | |||
if (!controller_->computeVelocityCommand(target_pose.pose,robot_pose.pose, cmd, backward)) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
You don't need to pass the robot pose to calculate the velocity, because the underlying control law generates it in the base frame.
if(initial_rotation_ && fabs(angle_to_goal) > initial_rotation_min_angle_){ | ||
command->twist = controller_->rotateToTarget(angle_to_goal); | ||
} | ||
else if (!controller_->computeVelocityCommand(target_pose.pose, robot_pose.pose, command->twist, dock_backwards_)) { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Ditto
{ | ||
std::lock_guard<std::mutex> lock(dynamic_params_lock_); | ||
cmd = control_law_->calculateRegularVelocity(pose, backward); | ||
cmd = control_law_->calculateRegularVelocity(pose, robot_pose, backward); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I'll try to explain them in detail here:
- calculateRegularVelocity(target_pose, current_pose, backward) is used to calculate the velocity from the current_pose.
- calculateRegularVelocity(target_pose, backward) is used to calculate the velocity from the origin (or base frame of the robot). This function internally calls calculateRegularVelocity(target_pose, {0,0,0}, backward). If current_pose is (0,0,0), the result function will be the same.
Internally, the velocitywould be calculated using ~ the difference between the two poses.
So if your target_pose is different from the robot_frame, you could do the following
robot_pose = getRobotPoseInFrame(target_frame)
calculateRegularVelocity(target_pose, robot_pose, backward)
OR
transform(target_pose, base_frame_)
calculateRegularVelocity(target_pose, backward)
The speed would be the same.
Because @Jakubach has removed the backward projection and the later transformation, they should use the 'full' calculateRegularVelocity instead of the 'simpler' one.
I hope my explanation is clear enough.
7d62a43
to
e02d9d7
Compare
Signed-off-by: Jakubach <[email protected]> Signed-off-by: jakubach <[email protected]> Signed-off-by: Jakubach <[email protected]>
Signed-off-by: Jakubach <[email protected]> Signed-off-by: jakubach <[email protected]> Signed-off-by: Jakubach <[email protected]>
Signed-off-by: Jakubach <[email protected]> Signed-off-by: jakubach <[email protected]> Signed-off-by: Jakubach <[email protected]>
Signed-off-by: jakubach <[email protected]> Signed-off-by: Jakubach <[email protected]>
Signed-off-by: Jakubach <[email protected]>
Signed-off-by: Jakubach <[email protected]>
…on#4744) * Pass IDLE to on_tick, use that for initialize condition Signed-off-by: redvinaa <[email protected]> * Fix battery sub creation bug Signed-off-by: redvinaa <[email protected]> --------- Signed-off-by: redvinaa <[email protected]> Signed-off-by: Jakubach <[email protected]>
…r= (ros-navigation#4753) default_value_ is an attribute of the Costmap2D class and should be copied along with the other attributes. Signed-off-by: Dylan De Coeyer <[email protected]> Signed-off-by: Jakubach <[email protected]>
… (ros-navigation#4711) * mppi parameters_handler: Improve verbose handling (ros-navigation#4704) The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake <[email protected]> * Address review comments. (ros-navigation#4704) * remove comments. * Use RCLCPP_DEBUG instead of INFO for low level messages. * Add test for trying to access parameters that are not declared. Signed-off-by: Mike Wake <[email protected]> * mppi parameters_handler: Improve static/dynamic/not defined logging (ros-navigation#4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted. Signed-off-by: Mike Wake <[email protected]> * mppi parameters_handler: populate SetParametersResult (ros-navigation#4704) Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult with the reasons for unsuccessful parameter setting, so that it may be propogated back to a set parameter client. The mechanism provides feedback when attempting to set undefined parameters, static parameters and could be used to validate dynamic parameters and provide a reason for rejection. NOTE: This changes public interface of ParametersHandler class. s/setDynamicParamCallback/setParamCallback s/addDynamicParamCallback/addParamCallback which takes a callback function that is able to populate a rcl_interfaces::msg::SetParametersResult. In order to indicate an unsuccessful parameter change and the reason, callback functions should append a "\n" to the reason before appending to if it is not empty. Signed-off-by: Mike Wake <[email protected]> * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake <[email protected]> --------- Signed-off-by: Mike Wake <[email protected]> Signed-off-by: Jakubach <[email protected]>
* Added collision detection for docking Signed-off-by: Alberto Tudela <[email protected]> * Minor fixes Signed-off-by: Alberto Tudela <[email protected]> * Improve collision while undocking and test Signed-off-by: Alberto Tudela <[email protected]> * Fix smoke testing Signed-off-by: Alberto Tudela <[email protected]> * Rename dock_collision_threshold Signed-off-by: Alberto Tudela <[email protected]> * Added docs and params Signed-off-by: Alberto Tudela <[email protected]> * Minor changes in README Signed-off-by: Alberto Tudela <[email protected]> --------- Signed-off-by: Alberto Tudela <[email protected]> Signed-off-by: Jakubach <[email protected]>
Signed-off-by: Jakubach <[email protected]>
Signed-off-by: Jakubach <[email protected]>
413bec7
to
40a2b0f
Compare
This pull request is in conflict. Could you fix it @Jakubach? |
Signed-off-by: Jakubach <[email protected]>
There's some test failures / linting issues reported in CI: https://app.circleci.com/pipelines/github/ros-navigation/navigation2/13004/workflows/13cb1e43-ec84-477d-9253-5d4464f09fb2/jobs/39130/tests Please build / test locally not in our CI, it makes this very difficult to review when we're collapsing threads due to the volume of mergify warnings about build failures :-) |
Basic Info
Description of contribution in a few bullet points
Description of documentation updates required from your changes
Future work that may be required in bullet points
For Maintainers: