Angled undocking (#151)

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` --------- Co-authored-by: Apehaenger <[email protected]>
ClemensElflein · Oct 16, 2024 · b1e1c1d · b1e1c1d
1 parent c463bce
commit b1e1c1d
Show file tree

Hide file tree

Showing 5 changed files with 119 additions and 14 deletions.
diff --git a/config/mower_config.schema.json b/config/mower_config.schema.json
@@ -274,6 +274,13 @@
           "description": "Distance in [m] to drive forward after docking.",
           "x-environment-variable": "OM_DOCKING_DISTANCE"
         },
+        "OM_DOCKING_APPROACH_DISTANCE": {
+          "type": "number",
+          "default": 1.5,
+          "title": "Docking Approach Distance",
+          "description": "Distance in [m] from the docking point to start docking. Has to be at least the length from the dock to your nav- or mow-area, otherwise docking will fail!",
+          "x-environment-variable": "OM_DOCKING_APPROACH_DISTANCE"
+        },
         "OM_DOCKING_EXTRA_TIME": {
           "type": "number",
           "default": 0.0,
@@ -301,6 +308,34 @@
           "description": "Distance in [m] to drive for undocking.",
           "x-environment-variable": "OM_UNDOCK_DISTANCE"
         },
+        "OM_UNDOCK_ANGLED_DISTANCE": {
+          "type": "number",
+          "title": "Undock Angled Distance",
+          "default": 0.0,
+          "description": "Distance in [m] to continue driving at an angle for undocking.",
+          "x-environment-variable": "OM_UNDOCK_ANGLED_DISTANCE"
+        },
+        "OM_UNDOCK_ANGLE": {
+          "type": "number",
+          "title": "Undock Angle",
+          "default": 0.0,
+          "description": "Angle in degrees for second stage undocking.",
+          "x-environment-variable": "OM_UNDOCK_ANGLE"
+        },
+        "OM_UNDOCK_FIXED_ANGLE": {
+          "type": "boolean",
+          "title": "Undock Fixed Angle",
+          "default": true,
+          "description": "True to fix the undock angle, false to vary it.",
+          "x-environment-variable": "OM_UNDOCK_FIXED_ANGLE"
+        },
+        "OM_UNDOCK_USE_CURVE": {
+          "type": "boolean",
+          "title": "Undock Use Curve",
+          "default": true,
+          "description": "True to use a curved second stage, false for a straight second stage.",
+          "x-environment-variable": "OM_UNDOCK_USE_CURVE"
+        },
         "OM_OUTLINE_COUNT": {
           "type": "number",
           "default": 4,

diff --git a/config/mower_config.sh.example b/config/mower_config.sh.example
@@ -118,21 +118,45 @@ export OM_USE_F9R_SENSOR_FUSION=False
 ################################
 ##    Mower Logic Settings    ##
 ################################
+
+# Undock:
+#
+# The first stage distance to drive for undocking. As a minimum it needs to clear the dock.
+# If the additional angled move isn't used then this needs to be large enough for the robot to have GPS reception.
+export OM_UNDOCK_DISTANCE=2.0
+#
+# 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.
+# Note - this section may still be driven without gps so don't expect high positional accuracy.
+# export OM_UNDOCK_ANGLED_DISTANCE=0.0
+#
+# The angle at which to drive for the additional distance (neg values are to the left of the dock, pos to the right).
+# export OM_UNDOCK_ANGLE=0.0
+#
+# If true will allways use the angle specified.
+# If false will vary the undocking angle between +abs(OM_UNDOCK_ANGLE) to -abs(OM_UNDOCK_ANGLE) on subsequent undocks to reduce grass wear.
+# export OM_UNDOCK_FIXED_ANGLE=True
+#
+# If true will use a curved second stage undock move, if false will use a straight undock move.
+# export OM_UNDOCK_USE_CURVE=True
+
+# Docking:
+#
+# Docking starts at this distance from the dock point.
+# Has to be at least the length from the dock to your nav- or mow-area, otherwise docking will fail!
+# export OM_DOCKING_APPROACH_DISTANCE=1.5
+#
 # The distance to drive forward AFTER reaching the second docking point
 export OM_DOCKING_DISTANCE=1.0
-
+#
 # Extra time (s) to continue docking after detecting voltage
 export OM_DOCKING_EXTRA_TIME=0.0
-
+#
 # How many times to retry docking before giving up.
 export OM_DOCKING_RETRY_COUNT=4
-
+#
 # Whether to attempt redocking if the voltage is no longer detected after docking.
 export OM_DOCKING_REDOCK=False
 
-# The distance to drive for undocking. This needs to be large enough for the robot to have GPS reception
-export OM_UNDOCK_DISTANCE=2.0
-
 # Uncomment, if you want to use the perimeter sensor of your Mowgli-type mower
 # for docking. Set it to 1 or 2 dependig on the signal selected on the docking
 # station (S1 or S2).

diff --git a/src/mower_logic/cfg/MowerLogic.cfg b/src/mower_logic/cfg/MowerLogic.cfg
@@ -5,7 +5,11 @@ from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, d
 gen = ParameterGenerator()
 
 gen.add("automatic_mode", int_t, 0, "0 - Manual, 1 - Semiautomatic, 2 - Automatic", 0, 0, 2)
-gen.add("undock_distance", double_t, 0, "Distance to drive back for undocking", 2, 0, 100)
+gen.add("undock_distance", double_t, 0, "Initial distance to drive back straight for undocking", 2, 0, 100)
+gen.add("undock_angled_distance", double_t, 0, "Second stage distance to drive back at angle for undocking", 0, 0, 100)
+gen.add("undock_angle", double_t, 0, "Angle to drive back for second stage undocking", 0, -90, 90)
+gen.add("undock_fixed_angle", bool_t, 0, "True to fix the undock angle, false to vary it", True)
+gen.add("undock_use_curve", bool_t, 0, "True to use a curved second stage, false for a straight second stage", True)
 gen.add("docking_distance", double_t, 0, "Distance to drive forward during docking", 2, 0, 100)
 gen.add("docking_approach_distance", double_t, 0, "Distance to approach docking point", 1.5, 0, 5)
 gen.add("docking_retry_count", int_t, 0, "How often should we retry docking", 4, 0, 50)

diff --git a/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/UndockingBehavior.cpp
@@ -17,6 +17,8 @@
 //
 #include "UndockingBehavior.h"
 
+#include "tf2_eigen/tf2_eigen.h"
+
 extern ros::ServiceClient dockingPointClient;
 extern actionlib::SimpleActionClient<mbf_msgs::ExePathAction> *mbfClientExePath;
 extern xbot_msgs::AbsolutePose getPose();
@@ -35,6 +37,8 @@ std::string UndockingBehavior::state_name() {
 }
 
 Behavior *UndockingBehavior::execute() {
+  static bool rng_seeding_required = true;
+
   // get robot's current pose from odometry.
   xbot_msgs::AbsolutePose pose = getPose();
   tf2::Quaternion quat;
@@ -47,13 +51,46 @@ Behavior *UndockingBehavior::execute() {
 
   nav_msgs::Path path;
 
-  int undock_point_count = config.undock_distance * 10.0;
-  for (int i = 0; i < undock_point_count; i++) {
-    geometry_msgs::PoseStamped docking_pose_stamped_front;
-    docking_pose_stamped_front.pose = pose.pose.pose;
-    docking_pose_stamped_front.header = pose.header;
-    docking_pose_stamped_front.pose.position.x -= cos(yaw) * (i / 10.0);
-    docking_pose_stamped_front.pose.position.y -= sin(yaw) * (i / 10.0);
+  geometry_msgs::PoseStamped docking_pose_stamped_front;
+  docking_pose_stamped_front.pose = pose.pose.pose;
+  docking_pose_stamped_front.header = pose.header;
+
+  const int straight_undock_point_count = 3;  // The FTC planner requires at least 3 points to work
+  double incremental_distance = config.undock_distance / straight_undock_point_count;
+  for (int i = 0; i < straight_undock_point_count; i++) {
+    docking_pose_stamped_front.pose.position.x -= cos(yaw) * incremental_distance;
+    docking_pose_stamped_front.pose.position.y -= sin(yaw) * incremental_distance;
+    path.poses.push_back(docking_pose_stamped_front);
+  }
+
+  double angle;
+  if (config.undock_fixed_angle) {
+    angle = config.undock_angle * M_PI / 180.0;
+    ROS_INFO_STREAM("Fixed angle undock: " << config.undock_angle);
+  } else {
+    // seed based on first undock time rather than boot so should be ok even without RTC
+    if (rng_seeding_required) {
+      srand(ros::Time::now().toSec());
+      ROS_INFO_STREAM("Random angle undock: Seeded rand()");
+      rng_seeding_required = false;
+    }
+    double random_number = ((double)rand() / RAND_MAX) * 2.0 - 1.0;
+    double random_angle_deg = abs(config.undock_angle) * random_number;
+    ROS_INFO_STREAM("Random angle undock: " << random_angle_deg);
+    angle = random_angle_deg * M_PI / 180.0;
+  }
+
+  const int angled_undock_point_count = 10;
+  incremental_distance = config.undock_angled_distance / angled_undock_point_count;
+  for (int i = 0; i < angled_undock_point_count; i++) {
+    double orientation = yaw + angle * (config.undock_use_curve ? ((i + 1.0) / angled_undock_point_count) : 1);
+
+    docking_pose_stamped_front.pose.position.x -= cos(orientation) * incremental_distance;
+    docking_pose_stamped_front.pose.position.y -= sin(orientation) * incremental_distance;
+
+    tf2::Quaternion q;
+    q.setRPY(0.0, 0.0, orientation);
+    docking_pose_stamped_front.pose.orientation = tf2::toMsg(q);
     path.poses.push_back(docking_pose_stamped_front);
   }
 

diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch
@@ -15,6 +15,7 @@
     <node pkg="mower_logic" type="mower_logic" name="mower_logic" output="screen">
         <param name="automatic_mode" value="$(optenv OM_AUTOMATIC_MODE 0)"/>
         <param name="docking_distance" value="$(env OM_DOCKING_DISTANCE)"/>
+        <param name="docking_approach_distance" value="$(optenv OM_DOCKING_APPROACH_DISTANCE 1.5)"/>
         <param name="docking_extra_time" value="$(optenv OM_DOCKING_EXTRA_TIME 0)"/>
         <param name="docking_retry_count" value="$(optenv OM_DOCKING_RETRY_COUNT 4)"/>
         <param name="docking_redock" value="$(optenv OM_DOCKING_REDOCK False)"/>
@@ -41,6 +42,10 @@
         <param name="rain_mode" value="$(optenv OM_RAIN_MODE 0)"/>
         <param name="rain_delay_minutes" value="$(optenv OM_RAIN_DELAY_MINUTES 30)"/>
         <param name="rain_check_seconds" value="$(optenv OM_RAIN_CHECK_SECONDS 20)"/>
+        <param name="undock_angled_distance" value="$(optenv OM_UNDOCK_ANGLED_DISTANCE 0.0)"/>
+        <param name="undock_angle" value="$(optenv OM_UNDOCK_ANGLE 0.0)"/>
+        <param name="undock_fixed_angle" value="$(optenv OM_UNDOCK_FIXED_ANGLE True)"/>
+        <param name="undock_use_curve" value="$(optenv OM_UNDOCK_USE_CURVE True)"/>
     </node>
     <node pkg="slic3r_coverage_planner" type="slic3r_coverage_planner" name="slic3r_coverage_planner" output="screen"/>