Skip to content
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.

Sign up for GitHub

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

Jump to bottom

Do not abort current behaviour after emergency is cleared #86

Merged
merged 4 commits into from
May 25, 2024
Merged

Do not abort current behaviour after emergency is cleared #86

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3ab241b
Do not abort current behaviour after emergency is cleared
2m Sep 11, 2023
7a52f47
Pause/continue on emergency
olliewalsh Mar 12, 2024
042a4dd
Move emergency pause/continue logic to checkSafety
olliewalsh May 20, 2024
d7c49c2
Re-work pause logic to handle emergency pause
olliewalsh May 23, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 11 additions & 20 deletions src/mower_logic/src/mower_logic/behaviors/Behavior.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,11 @@ enum eAutoMode {
AUTO = 2
};

enum pauseType {
PAUSE_MANUAL = 0b1,
PAUSE_EMERGENCY = 0b10
};

struct sSharedState {
// True, if the semiautomatic task is still in progress
bool active_semiautomatic_task;
Expand All @@ -48,8 +53,7 @@ class Behavior {
std::atomic<bool> aborted;
std::atomic<bool> paused;

std::atomic<bool> requested_continue_flag;
std::atomic<bool> requested_pause_flag;
std::atomic<u_int8_t> requested_pause_flag;

std::atomic<bool> isGPSGood;
std::atomic<uint8_t> sub_state;
Expand Down Expand Up @@ -82,26 +86,14 @@ class Behavior {
isGPSGood = isGood;
}

void requestContinue()
{
requested_continue_flag = true;
}

void requestPause()
void requestContinue(pauseType reason = pauseType::PAUSE_MANUAL)
{
requested_pause_flag = true;
requested_pause_flag &= ~reason;
}

void setPause()
void requestPause(pauseType reason = pauseType::PAUSE_MANUAL)
{
paused = true;
}

void setContinue()
{
paused = false;
requested_continue_flag = false;
requested_pause_flag = false;
requested_pause_flag |= reason;
}

void start(mower_logic::MowerLogicConfig &c, std::shared_ptr<sSharedState> s) {
Expand All @@ -112,8 +104,7 @@ class Behavior {
ROS_INFO_STREAM("--------------------------------------");
aborted = false;
paused = false;
requested_continue_flag = false;
requested_pause_flag = false;
requested_pause_flag = 0;
this->config = c;
this->shared_state = std::move(s);
startTime = ros::Time::now();
Expand Down
47 changes: 31 additions & 16 deletions src/mower_logic/src/mower_logic/behaviors/MowingBehavior.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -129,8 +129,8 @@ void MowingBehavior::update_actions() {
}

// pause / resume switch. other actions are always available
actions[0].enabled = !paused && !requested_pause_flag;
actions[1].enabled = paused && !requested_continue_flag;
actions[0].enabled = !(requested_pause_flag & pauseType::PAUSE_MANUAL);
actions[1].enabled = requested_pause_flag & pauseType::PAUSE_MANUAL;

registerActions("mower_logic:mowing", actions);
}
Expand Down Expand Up @@ -272,31 +272,44 @@ bool MowingBehavior::execute_mowing_plan() {
////////////////////////////////////////////////
if (requested_pause_flag)
{ // pause was requested
this->setPause(); // set paused=true
update_actions();
paused = true;
mowerEnabled = false;
while (!requested_continue_flag) // while not asked to continue, we wait
u_int8_t last_requested_pause_flags = 0;
while (requested_pause_flag) // while emergency and/or manual pause not asked to continue, we wait
{
ROS_INFO_STREAM("MowingBehavior: PAUSED (waiting for CONTINUE)");
if (last_requested_pause_flags != requested_pause_flag) {
update_actions();
}
last_requested_pause_flags = requested_pause_flag;

std::string pause_reason = "";
if (requested_pause_flag & pauseType::PAUSE_EMERGENCY) {
pause_reason += "on EMERGENCY";
if (requested_pause_flag & pauseType::PAUSE_MANUAL) {
pause_reason += " and ";
}
}
if (requested_pause_flag & pauseType::PAUSE_MANUAL) {
pause_reason += "waiting for CONTINUE";
}
ROS_INFO_STREAM_THROTTLE(30, "MowingBehavior: PAUSED (" << pause_reason << ")");
ros::Rate r(1.0);
r.sleep();
}
// we will drop into paused, thus will also wait for /odom to be valid again
// we will drop into paused, thus will also wait for GPS to be valid again
}
if (paused)
{
paused_time = ros::Time::now();
mowerEnabled = false;
while (!this->hasGoodGPS()) // while no good GPS we wait
{
ROS_INFO_STREAM("MowingBehavior: PAUSED (" << (ros::Time::now()-paused_time).toSec() << "s) (waiting for /odom)");
ROS_INFO_STREAM("MowingBehavior: PAUSED (" << (ros::Time::now()-paused_time).toSec() << "s) (waiting for GPS)");
ros::Rate r(1.0);
r.sleep();
}
ROS_INFO_STREAM("MowingBehavior: CONTINUING");
this->setContinue();
paused = false;
update_actions();
mowerEnabled = true;
}


Expand Down Expand Up @@ -385,7 +398,7 @@ bool MowingBehavior::execute_mowing_plan() {
if (first_point_attempt_counter < config.max_first_point_attempts)
{
ROS_WARN_STREAM("MowingBehavior: (FIRST POINT) - Attempt " << first_point_attempt_counter << " / " << config.max_first_point_attempts << " Making a little pause ...");
this->setPause();
paused = true;
update_actions();
}
else
Expand All @@ -400,7 +413,7 @@ bool MowingBehavior::execute_mowing_plan() {
currentMowingPathIndex++;
first_point_trim_counter++;
first_point_attempt_counter = 0; // give it another <config.max_first_point_attempts> attempts
this->setPause();
paused = true;
update_actions();
}
else
Expand Down Expand Up @@ -522,9 +535,11 @@ bool MowingBehavior::execute_mowing_plan() {

// currentMowingPathIndex might be 0 if we never consumed one of the points, we advance at least 1 point
if( currentMowingPathIndex == 0) currentMowingPathIndex++;
ROS_INFO_STREAM("MowingBehavior: (MOW) PAUSED due to MBF Error at " << currentMowingPathIndex);
this->setPause();
update_actions();
if (!requested_pause_flag) {
ROS_INFO_STREAM("MowingBehavior: (MOW) PAUSED due to MBF Error at " << currentMowingPathIndex);
paused = true;
update_actions();
}
}
}
}
Expand Down
40 changes: 22 additions & 18 deletions src/mower_logic/src/mower_logic/mower_logic.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -224,19 +224,19 @@ bool setGPS(bool enabled) {
}


/// @brief If the BLADE Motor is not in the requested status (enabled),we call the
/// @brief If the BLADE Motor is not in the requested status (enabled),we call the
/// the mower_service/mow_enabled service to enable/disable. TODO: get feedback about spinup and delay if needed
/// @param enabled
/// @return
bool setMowerEnabled(bool enabled)
/// @param enabled
/// @return
bool setMowerEnabled(bool enabled)
{
const auto last_config = getConfig();

if (!last_config.enable_mower && enabled) {
// ROS_INFO_STREAM("om_mower_logic: setMowerEnabled() - Mower should be enabled but is hard-disabled in the config.");
enabled = false;
}

// status change ?
if (mowerEnabled != enabled)
{
Expand Down Expand Up @@ -317,8 +317,8 @@ void stopBlade()


/// @brief Stop BLADE motor and any movement
/// @param emergency
void setEmergencyMode(bool emergency)
/// @param emergency
void setEmergencyMode(bool emergency)
{
stopBlade();
stopMoving();
Expand Down Expand Up @@ -365,7 +365,7 @@ bool isGpsGood() {
}

/// @brief Called every 0.5s, used to control BLADE motor via mower_enabled variable and stop any movement in case of /odom and /mower/status outages
/// @param timer_event
/// @param timer_event
void checkSafety(const ros::TimerEvent &timer_event) {
const auto last_status = getStatus();
const auto last_config = getConfig();
Expand All @@ -374,20 +374,24 @@ void checkSafety(const ros::TimerEvent &timer_event) {
const auto status_time = getStatusTime();
const auto last_good_gps = getLastGoodGPS();


// call the mower
setMowerEnabled(currentBehavior != nullptr && currentBehavior->mower_enabled());

high_level_status.emergency = last_status.emergency;
high_level_status.is_charging = last_status.v_charge > 10.0;

// send to idle if emergency and we're not recording
if(last_status.emergency) {
if(currentBehavior != &AreaRecordingBehavior::INSTANCE && currentBehavior != &IdleBehavior::INSTANCE) {
abortExecution();
} else if(last_status.v_charge > 10.0) {
// emergency and docked and idle or area recording, so it's safe to reset the emergency mode, reset it. It's safe since we won't start moving in this mode.
setEmergencyMode(false);
if (currentBehavior != nullptr) {
if(last_status.emergency) {
currentBehavior->requestPause(pauseType::PAUSE_EMERGENCY);
if(currentBehavior == &AreaRecordingBehavior::INSTANCE || currentBehavior == &IdleBehavior::INSTANCE) {
if(last_status.v_charge > 10.0) {
// emergency and docked and idle or area recording, so it's safe to reset the emergency mode, reset it. It's safe since we won't start moving in this mode.
setEmergencyMode(false);
}
}
} else {
currentBehavior->requestContinue(pauseType::PAUSE_EMERGENCY);
}
}

Expand All @@ -401,10 +405,10 @@ void checkSafety(const ros::TimerEvent &timer_event) {
ROS_WARN_STREAM_THROTTLE(5, "om_mower_logic: EMERGENCY pose values stopped. dt was: " << (ros::Time::now() - pose_time));
return;
}

// check if status is current. if not, we have a problem since it contains wheel ticks and so on.
// Since these should never drop out, we enter emergency instead of "only" stopping
if (ros::Time::now() - status_time > ros::Duration(3))
if (ros::Time::now() - status_time > ros::Duration(3))
{
setEmergencyMode(true);
ROS_WARN_STREAM_THROTTLE(5, "om_mower_logic: EMERGENCY /mower/status values stopped. dt was: " << (ros::Time::now() - status_time));
Expand Down Expand Up @@ -505,7 +509,7 @@ bool highLevelCommand(mower_msgs::HighLevelControlSrvRequest &req, mower_msgs::H
}
break;
case mower_msgs::HighLevelControlSrvRequest::COMMAND_S2:
ROS_INFO_STREAM("COMMAND_S2");
ROS_INFO_STREAM("COMMAND_S2");
if(currentBehavior) {
currentBehavior->command_s2();
}
Expand Down
Loading