Add additional support for perimeter board.

src/mower_logic/CMakeLists.txt

@@ -174,6 +174,8 @@ add_executable(mower_logic
         src/mower_logic/behaviors/IdleBehavior.cpp
         src/mower_logic/behaviors/MowingBehavior.h
         src/mower_logic/behaviors/MowingBehavior.cpp
+        src/mower_logic/behaviors/PerimeterDocking.h
+        src/mower_logic/behaviors/PerimeterDocking.cpp
         src/mower_logic/behaviors/AreaRecordingBehavior.h
         src/mower_logic/behaviors/AreaRecordingBehavior.cpp
         )

src/mower_logic/cfg/MowerLogic.cfg

@@ -5,10 +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, 5)
-gen.add("docking_distance", double_t, 0, "Distance to drive forward during docking", 2, 0, 5)
+gen.add("undock_distance", double_t, 0, "Distance to drive back for undocking", 2, 0, 100)
+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, 1, 10)
+gen.add("perimeter_signal",int_t, 0, "Which perimeter signal should be used? 0-None, positive number gives signal number and uses counterclockwise docking, negative numbers use clockwise docking", 0, -2, 2)
 gen.add("outline_count", int_t, 0, "Outline count to mow before filling", 3, 0, 255)
 gen.add("outline_overlap_count", int_t, 0, "Outlines to overlap with fill", 0, 0, 255)
 gen.add("outline_offset", double_t, 0, "Additional outer outline offset, use positive values for safety, negative values to enlarge the area", 0.0, -1.0, 1.0)

src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp

@@ -15,6 +15,7 @@
 //
 //
 #include "DockingBehavior.h"
+#include "PerimeterDocking.h"
 
 extern ros::ServiceClient dockingPointClient;
 extern actionlib::SimpleActionClient<mbf_msgs::MoveBaseAction> *mbfClient;
@@ -209,6 +210,9 @@ Behavior *DockingBehavior::execute() {
     inApproachMode = false;
     setGPS(false);
 
+    if (PerimeterSearchBehavior::configured(config))
+      return &PerimeterSearchBehavior::INSTANCE;
+
     bool docked = dock_straight();
 
     if (!docked) {

src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp

@@ -15,6 +15,7 @@
 //
 //
 #include "IdleBehavior.h"
+#include "PerimeterDocking.h"
 
 extern void stopMoving();
 extern void stopBlade();
@@ -77,9 +78,11 @@ Behavior *IdleBehavior::execute() {
         if (manual_start_mowing || ((automatic_mode || active_semiautomatic_task) && mower_ready)) {
             // set the robot's position to the dock if we're actually docked
             if(last_status.v_charge > 5.0) {
-                ROS_INFO_STREAM("Currently inside the docking station, we set the robot's pose to the docks pose.");
-                setRobotPose(docking_pose_stamped.pose);
-                return &UndockingBehavior::INSTANCE;
+              if (PerimeterUndockingBehavior::configured(config))
+                return &PerimeterUndockingBehavior::INSTANCE;
+              ROS_INFO_STREAM("Currently inside the docking station, we set the robot's pose to the docks pose.");
+              setRobotPose(docking_pose_stamped.pose);
+              return &UndockingBehavior::INSTANCE;
             }
             // Not docked, so just mow
             setGPS(true);