Angled undocking #151
Angled undocking #151
Conversation
| double angle; | ||
| if (config.undock_fixed_angle) | ||
| angle = config.undock_angle * (M_PI + M_PI) / 360.0; | ||
| else | ||
| angle = next_undock_angle; |
There was a problem hiding this comment.
How about this?
double angle = config.undock_fixed_angle ? config.undock_angle * M_PI / 180.0 : next_undock_angle;There was a problem hiding this comment.
With the random angle feature the if isn't as simple anymore, so I have not applied this change.
| for (int i = 0; i < undock_point_count; i++) { | ||
| double orientation; | ||
| if (config.undock_use_curve) | ||
| orientation = yaw + ((i + 1) * angle / undock_point_count); | ||
| else | ||
| orientation = yaw + angle; |
There was a problem hiding this comment.
How about this?
for (int i = 1; i <= undock_point_count; i++) {
double orientation = yaw + angle * (config.undock_use_curve ? i / undock_point_count : 1);There was a problem hiding this comment.
I converted this into a ternary expression, but if you don't mind I'd like to keep the i + 1 because I much prefer a 0-based loop instead of a 1-based loop.
| next_undock_angle = angle + (abs(config.undock_angle) * (M_PI + M_PI) / 360.0) / 5.0; | ||
| if (next_undock_angle > abs(config.undock_angle) * (M_PI + M_PI) / 360.0) | ||
| next_undock_angle = -abs(config.undock_angle) * (M_PI + M_PI) / 360.0; |
There was a problem hiding this comment.
const double undock_angle_rad = abs(config.undock_angle) * M_PI / 180.0;
...
next_undock_angle = angle + undock_angle_rad / 5.0;
if (next_undock_angle > undock_angle_rad) {
next_undock_angle = -undock_angle_rad;
}There was a problem hiding this comment.
This code has been replaced with the random angle code (1ee783b), so there's nothing to change anymore.
|
I added some suggestions to make the code more crisp. I didn't check completely whether it makes sense, I think you tested that emperically. 😉 |
|
Pete_MI632 had some changes which I have incorporated. I split them into separate commits to make the review easier: 7acbeae 1ee783b b951aa7 I have then also applied your proposals as far as they were still valid. I'm marking this as a draft though until I have empirically tested it again, it's raining currently.
|
| @@ -37,7 +37,7 @@ std::string UndockingBehavior::state_name() { | |||
| } | |||
|
|
|||
| Behavior *UndockingBehavior::execute() { | |||
| static double next_undock_angle = 0.0; | |||
| static bool seedRqd = true; | |||
There was a problem hiding this comment.
The name should use under_scores instead of camelCase like the remaining code and "rqd" is unclear. Maybe random_seeded = true and then reverse the conditions?
There was a problem hiding this comment.
I guess that means "required", so how about rng_seeding required?
src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
Outdated
Show resolved
Hide resolved
src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
Outdated
Show resolved
Hide resolved
src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
Outdated
Show resolved
Hide resolved
|
|
||
| undock_point_count = config.undock_angled_distance * 10.0; | ||
| undock_point_count = 10; |
There was a problem hiding this comment.
That's because it's actually two different "configuration" values that just happened to be both 10. I split them up.
src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
Outdated
Show resolved
Hide resolved
|
I think there's a button somewhere to mark a conversation as resolved 😉 |
|
Where I work that's reserved for the reviewer. |
|
I see. Well, my opinion is that I did review the code, even if I have no push permissions and nobody needs to follow my ideas. 😁 For me it's easier to see whether anything needs further discussion if the "done" or "won't do" boxes are minimized (they can still be expanded at any time). |
|
Real life test was successful as well. |
|
Cool PR! Like these undocking enhancement variations ;-) Tested today without config settings as well as with config settings. What I didn't tested is However, beside @rovo89 's good comments, I think we need to change/add some tiny stuff before merging:
I know How are your thoughts? |
This adds the ability to drive backwards in a straight line or curve after undocking, optionally with a random angle. This can be used to reduce grass wear from undocking and also to get to an area with better GPS reception. This introduces 4 new environment variables for configuration: - OM_UNDOCK_ANGLED_DISTANCE: The additional second stage distance to drive at an angle for undocking. This needs to be large enough for the robot to have GPS reception. This section may still be driven without GPS so don't expect high positional accuracy. - OM_UNDOCK_ANGLE: The angle at which to drive for the additional distance (neg values are to the left of the dock, pos to the right). - OM_UNDOCK_FIXED_ANGLE: If false will vary the undocking angle between +abs(OM_UNDOCK_ANGLE) to -abs(OM_UNDOCK_ANGLE) on subsequent undocks to reduce grass wear. - OM_UNDOCK_USE_CURVE: If true will use a curved second stage undock move, if false will use a straight undock move. This also restores one more config variable that already existed but was erroneously removed in a previous commit: OM_DOCKING_APPROACH_DISTANCE
(works better on shorter paths)
by using float arithmetic instead of integer arithmetic
AndreKR
force-pushed
the
angled-undocking
branch
from
8668c85
to
60c6553
Compare
|
I have added the parameters and the defaults to the various schema and config files. Pete_MI632 had actually provided me with an example config.sh but I had no idea where to put it.
I don't think there is a logical issue with zero approach distance, it would just mean the robot drives to right in front of the dock with no chance to correct its heading. If your dock is at the end of a long narrow navigation area it might actually work. |
|
Thanks a lot for the corrections! Regarding zero approach distance: Unfortunately I can't test today ;-( |
|
Found some time today for testing ;-) Openmower service exit with error 125 :-/ Fixed in local... Tested with |
Oh, I see, the other values use
Hm, yeah, so ideally we should have a different minimum there. I googled a bit and those parameters don't seem to support an exclusive min value, so we would have to come up with a minimum that isn't 0. Something like 1cm? |
Thanks!
Well I think we should find a reasonable value. If I understand the functionality of "docking approach instance" in the right way, then it's the distance of the the mower to the dock when it tries to (re-)dock. Are you able to run a simulator? |
|
Yes, when you click "Record docking" the mower records its current GPS coordinates and heading. Then whenever it wants to go home, it will drive to a point that lays I'm currently using an I haven't gotten the simulator to work because it needs(?) rviz which in turn needs OpenGL and crashes when I try to run it over a forwarded X11 connection. |
|
Thanks for the description. So the question is: Does If I also don't know how to run the simulator :-( |
Yes, I think
Good point. In fact, I said above
without realizing this also restricts the minimum approach distance. So the approach distance should basically be about the length of the robot (it doesn't turn around its center). |
|
Did 2 more tests today. So, I thought it's a <1.0 thingy and tested with Its worthless to poke around this way 😥 . It most likely also depends on |
|
😢 You don't know in what way the calculation fails when it's too small? Shall we put
I really don't like having hidden config variables, the fact that it was missing from the example config felt very much like a bug. So before we remove it again, we should just put comments like "this can cause docking to fail if it's too small". |
I'm sorry "no" 😢 Not clever enough 😟 . May be @rovo89 ?
Not a good idea in my mind. It's forgotten 2 seconds after changed and saved. |
I didn't follow your conversation... In the highlighted lines, I think it's just about navigating to a certain point. I really lack knowledge in that the FTC Planner is doing and when it will give up. Could it be something simply like if the target point isn't within a navigation/mowing area and therefore not reachable? I generally got the simulator running. It's been a few weeks since I last use it though. Also rviz is very slow in my scenario, but I can see the simulated mower move in the official app and the unofficial GUI. |
@AndreKR mainly search for a save minimum I guess that the reasoning is somewhere here: https://github.com/AndreKR/open_mower_ros/blob/65382e8f24ef353c618c05545571aee10d9e41ca/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp#L45-L46 , but I unfortunately have been @ work while they talked in school about sin, cos, yaw, and the like 😟 |
|
My understanding is that it simply calculates the target coordinates by starting from So I doubt that the calculation is wrong, I think there's a problem navigating to the calculated point. Again, how far away is your docking station from the lawn? Have you covered it with a navigation area? |
|
Oooh, that would make sense... you probably recorded your mowing/navigation area by passing the dock, right? So an approach point far away from the dock would be within the navigation area, but an approach point too close would be outside of it. Does it fail going from the lawn to the approach point or does it fail going from the approach point to the dock point? |
No, I never covered any of my docks with a navigation area.
It fail immediately after pressing home (i.e. while mowing). Press Homes -> mower stops, tries to calculate whatever docking and abort docking (but only if |
|
I'm not too sure where in that screenshot the Anyway, I'm pretty sure that every point that you want to reach with the FTC Planner must be within the mowing/navigation areas. It will consider the cost map to drive around obstacles etc. With the default It would be easy to test this theory by recording a dummy docking station. The drive-straight-forward part won't succeed obviously. but it should navigate to it. Or just carry the mower while recording a navigation area 😆 |
|
Or even easier, just create the navigation area with Cedric's GUI by clicking on the points. |
Mee too 🤔 that's why I asked a couple of post before if
That's the best fitting explanation till now and also would make sense to my failed as well working tries 👍 . |
|
Checked the sources once more and @rovo89's description fit best. It is the home/dock position we've talked of. I've been that insolent and changed 2 files directly in your branch, hope it's okay. Please validate. |
|
Quite thanks for this PR 👍 . Just merged. |
This adds the ability to drive backwards in a straight line or curve after undocking, optionally with a random angle.
This can be used to reduce grass wear from undocking and also to get to an area with better GPS reception.
This introduces 4 new environment variables for configuration:
OM_UNDOCK_ANGLED_DISTANCE: The additional second stage distance to drive at an angle for undocking. This needs to be large enough for the robot to have GPS reception. This section may still be driven without GPS so don't expect high positional accuracy.OM_UNDOCK_ANGLE: The angle at which to drive for the additional distance (neg values are to the left of the dock, pos to the right).OM_UNDOCK_FIXED_ANGLE: If false will vary the undocking angle between +abs(OM_UNDOCK_ANGLE) to -abs(OM_UNDOCK_ANGLE) on subsequent undocks to reduce grass wear.OM_UNDOCK_USE_CURVE: If true will use a curved second stage undock move, if false will use a straight undock move.This also restores one more config variable that already existed but was erroneously removed in a previous commit:
OM_DOCKING_APPROACH_DISTANCE