diff --git a/src/mower_logic/CMakeLists.txt b/src/mower_logic/CMakeLists.txt index 728213d3..42168fc5 100644 --- a/src/mower_logic/CMakeLists.txt +++ b/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 ) diff --git a/src/mower_logic/cfg/MowerLogic.cfg b/src/mower_logic/cfg/MowerLogic.cfg index 58699a10..1228767a 100755 --- a/src/mower_logic/cfg/MowerLogic.cfg +++ b/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) diff --git a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp index 9dcb6f9a..4e649634 100644 --- a/src/mower_logic/src/mower_logic/behaviors/DockingBehavior.cpp +++ b/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 *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) { diff --git a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp b/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp index 94045c5d..65e60f9b 100644 --- a/src/mower_logic/src/mower_logic/behaviors/IdleBehavior.cpp +++ b/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); diff --git a/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp new file mode 100644 index 00000000..a8917e9a --- /dev/null +++ b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.cpp @@ -0,0 +1,384 @@ +#include "PerimeterDocking.h" +#include "IdleBehavior.h" +#include "mower_msgs/Perimeter.h" +#include "mower_msgs/PerimeterControlSrv.h" + +#define MIN_SIGNAL 5 +#define SEARCH_SPEED 0.1 +#define ANGULAR_SPEED 0.5 + +/* Distance from center to outer coil */ +#define COIL_Y_OFFSET 0.11 +/* Distance from rear axis to coils */ +#define COIL_X_OFFSET 0.40 + +#define FOLLOW_STATE_FOLLOW 0 +#define FOLLOW_STATE_TURN_IN 1 +#define FOLLOW_STATE_TURN_OUT 2 + +extern ros::NodeHandle *n; +extern ros::Publisher cmd_vel_pub; +extern mower_msgs::Status getStatus(); +extern void setGPS(bool enabled); + +static ros::Subscriber perimeterSubscriber; +static ros::ServiceClient perimeterClient; + +static mower_msgs::Perimeter lastPerimeter; +static int perimeterUpdated=0; +static int direction; // 1 if we approach counter clockwise, -1 for clockwise +/* Sensitivty of the coils, sign makes calibration*rawSignal positive inside the perimeter */ +static double calibrationLeft,calibrationRight,maxCenter; +static int signCenter; + + +PerimeterSearchBehavior PerimeterSearchBehavior::INSTANCE; +PerimeterDockingBehavior PerimeterDockingBehavior::INSTANCE; +PerimeterUndockingBehavior PerimeterUndockingBehavior::INSTANCE; +PerimeterMoveToGpsBehavior PerimeterMoveToGpsBehavior::INSTANCE; + +static void perimeterReceived(const mower_msgs::Perimeter::ConstPtr &msg) { + lastPerimeter=*msg; + perimeterUpdated=1; +} + +static Behavior* shutdownConnections() { + mower_msgs::PerimeterControlSrv p; + p.request.listenOn=0; + if (perimeterClient.call(p)) { + ROS_INFO("Perimeter deactivated."); + } else { + ROS_ERROR("Failed to deactivate perimeter."); + } + perimeterSubscriber.shutdown(); + perimeterClient.shutdown(); + return &IdleBehavior::INSTANCE; +} + +static int isPerimeterUpdated() { + if (perimeterUpdated) { + perimeterUpdated=0; + return 1; + } + return 0; +} + +static float innerSignal() { + return direction>0 ? lastPerimeter.left*calibrationLeft : lastPerimeter.right*calibrationRight; +} + +static float outerSignal() { + return direction>0 ? lastPerimeter.right*calibrationRight : lastPerimeter.left*calibrationLeft; +} + +int PerimeterSearchBehavior::configured(const mower_logic::MowerLogicConfig &config) { + return config.perimeter_signal!=0; +} + +std::string PerimeterSearchBehavior::state_name() { + return "DOCKING"; +} + +Behavior* PerimeterSearchBehavior::execute() { + if (!setupConnections()) return shutdownConnections(); + ros::Rate rate(10); + int tries=100; + int toFind=5; + /* Wait ten seconds for initial perimeter signal */ + while (tries-- && toFind) { + if (isPerimeterUpdated()) { + toFind--; + } + rate.sleep(); + } + + if (toFind) { + ROS_ERROR("Failed to activate perimeter"); + return shutdownConnections(); + } + + /* We expect to be currently outside the perimeter wire => negative signal */ + calibrationLeft=calibrationRight=signCenter= + lastPerimeter.left<0 ? 1 : -1; // Determine polarity of the cable. + + if (innerSignal()>-MIN_SIGNAL || outerSignal()>-MIN_SIGNAL) { + ROS_ERROR("Signal too weak."); + return shutdownConnections(); + } + calibrationLeft/=fabs(lastPerimeter.left); + maxCenter=fabs(lastPerimeter.center); + calibrationRight/=fabs(lastPerimeter.right); + + geometry_msgs::Twist vel; + /* Move straight until one of the signals changes to positive (inside) */ + vel.angular.z=0; + vel.linear.x=SEARCH_SPEED; + tries=10; // One second to next perimeter msg. + while (tries-->0) { + cmd_vel_pub.publish(vel); + rate.sleep(); + if (isPerimeterUpdated()) { + tries=10; + if (innerSignal()>0 || outerSignal()>0) break; + } + } + + if (!tries) { + ROS_ERROR("Signal timeout"); + return shutdownConnections(); + } + + /* Move further until the wheels are over the perimeter (approx. 30 cm) */ + for (tries=20; tries-->0; ) { + cmd_vel_pub.publish(vel); + rate.sleep(); + } + + return &PerimeterDockingBehavior::INSTANCE; +} + +int PerimeterUndockingBehavior::configured(const mower_logic::MowerLogicConfig &config) { + return config.perimeter_signal!=0 && config.undock_distance>1.0; +} + +std::string PerimeterUndockingBehavior::state_name() { + return "UNDOCKING"; +} + +Behavior* PerimeterUndockingBehavior::execute() { + if (!setupConnections()) return shutdownConnections(); + ros::Rate rate(10); + int tries=100; + int toFind=5; + /* Wait ten seconds for initial perimeter signal */ + while (tries-- && toFind) { + if (isPerimeterUpdated()) { + toFind--; + } + rate.sleep(); + } + + if (toFind) { + ROS_ERROR("Failed to activate perimeter"); + return shutdownConnections(); + } + + geometry_msgs::Twist vel; + /* Move straight back 0.5 m */ + vel.angular.z=0; + vel.linear.x=-SEARCH_SPEED; + double travelled=0; + while (travelled<0.5) { + cmd_vel_pub.publish(vel); + rate.sleep(); + travelled+=rate.expectedCycleTime().toSec()*SEARCH_SPEED; + } + + // Determine polarity of the cable. + calibrationLeft=calibrationRight=signCenter=1; + if (innerSignal()<0) calibrationLeft=calibrationRight=signCenter=-1; + + float maxLeft=lastPerimeter.left*calibrationLeft; + maxCenter=fabs(lastPerimeter.center); + float maxRight=lastPerimeter.right*calibrationRight;; + + /* Now make a 180 degree turn inwards */ + vel.angular.z=direction*ANGULAR_SPEED; + vel.linear.x=0; + tries=240; + while (innerSignal()>0 && --tries) { + cmd_vel_pub.publish(vel); + rate.sleep(); + if (isPerimeterUpdated()) { + float x=lastPerimeter.left*calibrationLeft; + if (x>maxLeft) maxLeft=x; + x=fabs(lastPerimeter.center); + if (x>maxCenter) maxCenter=x; + x=lastPerimeter.right*calibrationRight; + if (x>maxRight) maxRight=x; + } + } + if (!tries) { + ROS_ERROR("Could not turn inwards"); + return shutdownConnections(); + } + + if (maxLeftconfig.docking_distance) { + ROS_WARN("Travelled %.f meters before reaching the station",travelled); + return &IdleBehavior::INSTANCE; + } + if (getStatus().v_charge>5.0) { + chargeSeen++; + if (chargeSeen>=2) { + chargeSeen=0; + return &IdleBehavior::INSTANCE; + } + } else { + chargeSeen=0; + } + return NULL; +} + +Behavior* PerimeterFollowBehavior::execute() { + ros::Rate rate(10); + geometry_msgs::Twist vel; + travelled=0; + int state=FOLLOW_STATE_FOLLOW; + double travelTimeSinceUpdate=0; + double lastAlpha0=0; + int tries=0; + perimeterUpdated=1; // Use first measurement + Behavior* toReturn; + double drift=0; // The angular velocity of the mower, if we want to go strait on. + double averageInterval=5; // Average five seconds. + while (ros::ok() && !(toReturn=arrived())) { + if (!isPerimeterUpdated()) { + cmd_vel_pub.publish(vel); + rate.sleep(); + if (tries && --tries==0) { + ROS_ERROR("Timeout of action %d",state); + toReturn=&IdleBehavior::INSTANCE; + break; + } + double d=rate.expectedCycleTime().toSec(); + travelled+=d*vel.linear.x; + travelTimeSinceUpdate+=d; + continue; + } + /* Turn into direction of perimeter */ + if (innerSignal()<0) { + /* Inner coil is outside */ + vel.linear.x=0; + vel.angular.z=ANGULAR_SPEED*direction; + if (state!=FOLLOW_STATE_TURN_IN) { + tries=240; + state=FOLLOW_STATE_TURN_IN; + } + } else if (outerSignal()>0) { + /* Outer coil is inside */ + vel.linear.x=0; + vel.angular.z=-ANGULAR_SPEED*direction; + if (state!=FOLLOW_STATE_TURN_OUT) { + tries=240; + state=FOLLOW_STATE_TURN_OUT; + } + } else { + vel.linear.x=SEARCH_SPEED; + if (fabs(lastPerimeter.center)>maxCenter) { + maxCenter=fabs(lastPerimeter.center); + } + double c=lastPerimeter.center*signCenter; + /* Signal of the center coil is proportional to distance from the wire */ + /* y0: Position of the wire in mower coordinates */ + double y0=-direction*COIL_Y_OFFSET*c/maxCenter; + double alpha0=y0/COIL_X_OFFSET; // deflection + if (state!=FOLLOW_STATE_FOLLOW) { + state=FOLLOW_STATE_FOLLOW; + tries=0; + } else { + if (travelTimeSinceUpdate>0) { + double d0=-vel.angular.z+(alpha0-lastAlpha0)/travelTimeSinceUpdate; /* rad/s */ + double f=exp(-travelTimeSinceUpdate/averageInterval); + drift=drift*f+d0*(1-f); + } + } + vel.angular.z=drift+alpha0/2; //Correct deflection within 2 seconds + if (vel.angular.z>ANGULAR_SPEED) vel.angular.z=ANGULAR_SPEED; + else if (vel.angular.z<-ANGULAR_SPEED) vel.angular.z=-ANGULAR_SPEED; + travelTimeSinceUpdate=0; + lastAlpha0=alpha0; + } + } + /* And stop */ + vel.linear.x=0; + vel.angular.z=0; + cmd_vel_pub.publish(vel); + shutdownConnections(); + return toReturn; +} + +void PerimeterBase::enter() { + paused = aborted = false; +} + +void PerimeterBase::exit() {} + +void PerimeterBase::reset() {} + +uint8_t PerimeterBase::get_sub_state() { + return 1; +} + +uint8_t PerimeterBase::get_state() { + return mower_msgs::HighLevelStatus::HIGH_LEVEL_STATE_AUTONOMOUS; +} + +bool PerimeterBase::needs_gps() { + return false; +} + +bool PerimeterBase::mower_enabled() { + // No mower during docking + return false; +} + +void PerimeterBase::command_home() {} + +void PerimeterBase::command_start() {} + +void PerimeterBase::command_s1() {} + +void PerimeterBase::command_s2() {} + +bool PerimeterBase::redirect_joystick() { + return false; +} + +void PerimeterBase::handle_action(std::string action) {} + +/** + * @return success + */ +int PerimeterBase::setupConnections() { + perimeterSubscriber=n->subscribe("/mower/perimeter",0,perimeterReceived,ros::TransportHints().tcpNoDelay(true)); + perimeterClient=n->serviceClient("/mower_service/perimeter_listen"); + mower_msgs::PerimeterControlSrv p; + direction=config.perimeter_signal>0 ? 1 : -1; + p.request.listenOn=direction*config.perimeter_signal; + if (perimeterClient.call(p)) { + ROS_INFO("Perimeter activated"); + return 1; + } + ROS_ERROR("Failed to activate perimeter"); + return 0; +} + +void PerimeterMoveToGpsBehavior::enter() { + setGPS(true); +} + +std::string PerimeterMoveToGpsBehavior::state_name() { + return "UNDOCKING"; +} + +Behavior* PerimeterMoveToGpsBehavior::arrived() { + return travelled>=config.undock_distance-0.9 ? &MowingBehavior::INSTANCE : NULL; +} + diff --git a/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.h b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.h new file mode 100644 index 00000000..3fb02c98 --- /dev/null +++ b/src/mower_logic/src/mower_logic/behaviors/PerimeterDocking.h @@ -0,0 +1,112 @@ +// Created by Bodo Pfelzer on 28/10/23. +// Copyright (c) 2023 Bodo Pfelzer. All rights reserved. +// +// This work is licensed under a Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International License. +// +// Feel free to use the design in your private/educational projects, but don't try to sell the design or products based on it without getting my consent first. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +// +// +#ifndef SRC_PERIMETER_DOCKING_H +#define SRC_PERIMETER_DOCKING_H + +#include "Behavior.h" + +/* +#include +#include +#include +#include +#include "ros/ros.h" +#include +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" +#include "mower_msgs/Status.h" +#include +*/ + +class PerimeterBase : public Behavior { +public: + void enter() override; + void exit() override; + void reset() override; + bool needs_gps() override; + bool mower_enabled() override; + void command_home() override; + void command_start() override; + void command_s1() override; + void command_s2() override; + bool redirect_joystick() override; + uint8_t get_sub_state() override; + uint8_t get_state() override; + void handle_action(std::string action) override; +protected: + int setupConnections(); +}; + +class PerimeterFollowBehavior : public PerimeterBase { +public: + Behavior *execute() override; +protected: + /** + * @brief distance travelled. + */ + double travelled; + /** + * @brief We arrived and should continue with the returned Behavior. + */ + virtual Behavior* arrived()=0; +}; + +class PerimeterDockingBehavior : public PerimeterFollowBehavior { +private: + int chargeSeen; +public: + static PerimeterDockingBehavior INSTANCE; + std::string state_name() override; +protected: + Behavior* arrived() override; +}; + +class PerimeterSearchBehavior : public PerimeterBase { +public: + static PerimeterSearchBehavior INSTANCE; + /** + * Is usage configured? + */ + static int configured(const mower_logic::MowerLogicConfig &config); + + + std::string state_name() override; + Behavior *execute() override; +}; + +class PerimeterUndockingBehavior : public PerimeterBase { +public: + static PerimeterUndockingBehavior INSTANCE; + /** + * Is usage configured? + */ + static int configured(const mower_logic::MowerLogicConfig &config); + + std::string state_name() override; + Behavior *execute() override; +}; + +class PerimeterMoveToGpsBehavior : public PerimeterFollowBehavior { +public: + static PerimeterMoveToGpsBehavior INSTANCE; + std::string state_name() override; + void enter() override; +protected: + Behavior* arrived() override; +}; + +#endif //SRC_PERIMETER_DOCKING_H + diff --git a/src/mower_msgs/CMakeLists.txt b/src/mower_msgs/CMakeLists.txt index 23ccf79f..8a12a708 100644 --- a/src/mower_msgs/CMakeLists.txt +++ b/src/mower_msgs/CMakeLists.txt @@ -12,6 +12,7 @@ add_message_files( ImuRaw.msg ESCStatus.msg HighLevelStatus.msg + Perimeter.msg ) add_service_files( @@ -20,6 +21,7 @@ add_service_files( GPSControlSrv.srv MowerControlSrv.srv HighLevelControlSrv.srv + PerimeterControlSrv.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/src/mower_msgs/msg/Perimeter.msg b/src/mower_msgs/msg/Perimeter.msg new file mode 100644 index 00000000..9634f962 --- /dev/null +++ b/src/mower_msgs/msg/Perimeter.msg @@ -0,0 +1,5 @@ +# Will be published, if the mowgl/perimeter_listen has been called + +float32 left # Relative signal of the left coil +float32 center # Relative signal of the center coil +float32 right # Relative signal of the right coil diff --git a/src/mower_msgs/srv/PerimeterControlSrv.srv b/src/mower_msgs/srv/PerimeterControlSrv.srv new file mode 100644 index 00000000..ee9c04d2 --- /dev/null +++ b/src/mower_msgs/srv/PerimeterControlSrv.srv @@ -0,0 +1,2 @@ +uint8 listenOn # Listen for signal 1 or 2. Any other number disables perimeter sensing. +--- diff --git a/src/open_mower/config/mower_config.sh.example b/src/open_mower/config/mower_config.sh.example index 2510558e..eade45e8 100644 --- a/src/open_mower/config/mower_config.sh.example +++ b/src/open_mower/config/mower_config.sh.example @@ -106,6 +106,21 @@ export OM_DOCKING_DISTANCE=1.0 # 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). +# Normally docking is done counterclockwise. If you want to dock clockwise set +# it to -1 or -2. +# Set OM_DOCKING_DISTANCE to the number of meters, that must be travelled along +# the wire to reach to docking station. Add some meters to account for slipping. +# On the other hand do not add too much, as this value is a security measure used +# to stop the motor, if the mower does not hit the station exactly and tries to +# shove it away. +# Also set OM_UNDOCK_DISTANCE to the distance, that should be travelled along +# the wire during undocking. If this value is less than one meter, normal +# undocking will be used. +#OM_PERIMETER_SIGNAL=1 + # How many outlines should the mover drive. It's not recommended to set this below 4. export OM_OUTLINE_COUNT=4 diff --git a/src/open_mower/launch/open_mower.launch b/src/open_mower/launch/open_mower.launch index 6d775190..42f8cb05 100644 --- a/src/open_mower/launch/open_mower.launch +++ b/src/open_mower/launch/open_mower.launch @@ -12,6 +12,7 @@ +