Skip to content

Commit

Permalink
improving action results for sub actions called
Browse files Browse the repository at this point in the history
  • Loading branch information
@marinagmoreira
marinagmoreira committed Apr 11, 2024
1 parent dfacf7b commit 0d8b237
Show file tree
Hide file tree
Showing 2 changed files with 23 additions and 19 deletions.
20 changes: 11 additions & 9 deletions behaviors/dock/src/dock_nodelet.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -155,7 +155,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::DOCKING_SWITCHING_TO_ML_LOC,
SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::SWITCH_TO_ML_FAILED,
"Could not switch to mapped landmark localization");
"Could not switch to mapped landmark localization. " + err_msg_);
return STATE::UNDOCKED;
});
// [6]
Expand All @@ -168,7 +168,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::DOCKING_MOVING_TO_APPROACH_POSE,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::MOTION_APPROACH_FAILED;
err_msg_ = "Failed while moving to the approach pose";
err_msg_ = "Failed while moving to the approach pose. " + err_msg_;
Switch(LOCALIZATION_MAPPED_LANDMARKS);
return STATE::RECOVERY_SWITCHING_TO_ML_LOC;
});
Expand All @@ -182,7 +182,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::DOCKING_SWITCHING_TO_AR_LOC, SWITCH_FAILED,
[this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::SWITCH_TO_AR_FAILED,
"Could not switch to marker tracking localization");
"Could not switch to marker tracking localization. " + err_msg_);
return STATE::UNDOCKED;
});
// [8]
Expand All @@ -201,7 +201,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
}
if (event == MOTION_FAILED) {
err_ = RESPONSE::MOTION_COMPLETE_FAILED;
err_msg_ = "Motion failed trying to go to complete pose";
err_msg_ = "Motion failed trying to go to complete pose. " + err_msg_;
}
Move(APPROACH_POSE, ff_msgs::MotionGoal::PRECISION);
return STATE::RECOVERY_MOVING_TO_APPROACH_POSE;
Expand Down Expand Up @@ -230,7 +230,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::DOCKING_SWITCHING_TO_NO_LOC,
SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::SWITCH_TO_NO_FAILED,
"Could not turn localization off");
"Could not turn localization off. " + err_msg_);
return STATE::DOCKED;
});
// [14]
Expand All @@ -256,7 +256,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::UNDOCKING_SWITCHING_TO_ML_LOC,
SWITCH_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::SWITCH_TO_ML_FAILED;
err_msg_ = "Switch to Mapped Landmarks Failed";
err_msg_ = "Switch to Mapped Landmarks Failed. " + err_msg_;
Switch(LOCALIZATION_NONE);
return STATE::RECOVERY_SWITCHING_TO_NO_LOC;
});
Expand All @@ -271,7 +271,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::UNDOCKING_MOVING_TO_APPROACH_POSE,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::MOTION_APPROACH_FAILED,
"Could not move to approach point");
"Could not move to approach point. " + err_msg_);
return STATE::UNDOCKED;
});
// [21]
Expand Down Expand Up @@ -328,15 +328,15 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
// [28]
fsm_.Add(STATE::DOCKING_WAITING_FOR_SPIN_DOWN,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::PREP_DISABLE_FAILED, "Could not spin down motors");
Result(RESPONSE::PREP_DISABLE_FAILED, "Could not spin down motors. " + err_msg_);
Switch(LOCALIZATION_NONE);
return STATE::DOCKED;
});
// [29]
fsm_.Add(STATE::UNDOCKING_WAITING_FOR_SPIN_UP,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::PREP_ENABLE_FAILED;
err_msg_ = "Spin up was not successful";
err_msg_ = "Spin up was not successful. " + err_msg_;
Prep(ff_msgs::MotionGoal::OFF);
return STATE::RECOVERY_WAITING_FOR_SPIN_DOWN;
});
Expand Down Expand Up @@ -734,6 +734,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
case ff_util::FreeFlyerActionState::SUCCESS:
return fsm_.Update(SWITCH_SUCCESS);
default:
err_msg_ = "Result code " + result->fsm_result;
return fsm_.Update(SWITCH_FAILED);
}
}
Expand Down Expand Up @@ -854,6 +855,7 @@ class DockNodelet : public ff_util::FreeFlyerNodelet {
case ff_util::FreeFlyerActionState::SUCCESS:
return fsm_.Update(MOTION_SUCCESS);
default:
err_msg_ = "Result code " + result->fsm_result;
return fsm_.Update(MOTION_FAILED);
}
}
Expand Down
22 changes: 12 additions & 10 deletions behaviors/perch/src/perch_nodelet.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,39 +266,39 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::PERCHING_MOVING_TO_APPROACH_POSE,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::MOTION_FAILED;
err_msg_ = "Failed while moving to approach pose";
err_msg_ = "Failed while moving to approach pose. " + err_msg_;
Switch(LOCALIZATION_MAPPED_LANDMARKS);
return STATE::RECOVERY_SWITCHING_TO_ML_LOC;
});
// [21]
fsm_.Add(STATE::PERCHING_DEPLOYING_ARM,
ARM_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::ARM_FAILED;
err_msg_ = "Failed while deploying the arm";
err_msg_ = "Failed while deploying the arm. " + err_msg_;
Switch(LOCALIZATION_MAPPED_LANDMARKS);
return STATE::RECOVERY_SWITCHING_TO_ML_LOC;
});
// [22]
fsm_.Add(STATE::PERCHING_OPENING_GRIPPER,
ARM_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::ARM_FAILED;
err_msg_ = "Failed while opening the gripper";
err_msg_ = "Failed while opening the gripper. " + err_msg_;
Arm(ff_msgs::ArmGoal::ARM_STOW);
return STATE::RECOVERY_STOWING_ARM;
});
// [23]
fsm_.Add(STATE::PERCHING_MOVING_TO_COMPLETE_POSE,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::MOTION_FAILED;
err_msg_ = "Failed while moving to complete pose";
err_msg_ = "Failed while moving to complete pose. " + err_msg_;
Arm(ff_msgs::ArmGoal::GRIPPER_OPEN);
return STATE::RECOVERY_OPENING_GRIPPER;
});
// [24]
fsm_.Add(STATE::PERCHING_CLOSING_GRIPPER,
ARM_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::ARM_FAILED;
err_msg_ = "Failed while closing the gripper";
err_msg_ = "Failed while closing the gripper. " + err_msg_;
Arm(ff_msgs::ArmGoal::GRIPPER_OPEN);
return STATE::RECOVERY_OPENING_GRIPPER;
});
Expand All @@ -314,7 +314,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::PERCHING_WAITING_FOR_SPIN_DOWN,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::MOTION_FAILED,
"Spinning down propulsion failed");
"Spinning down propulsion failed. " + err_msg_);
return STATE::PERCHED;
});
// [27]
Expand All @@ -337,29 +337,29 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet {
fsm_.Add(STATE::UNPERCHING_WAITING_FOR_SPIN_UP,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::MOTION_FAILED,
"Spinning up propulsion failed");
"Spinning up propulsion failed. " + err_msg_);
return STATE::PERCHED;
});
// [30]
fsm_.Add(STATE::UNPERCHING_OPENING_GRIPPER,
ARM_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::ARM_FAILED,
"Gripper open failed");
"Gripper open failed. " + err_msg_);
return STATE::PERCHED;
});
// [31]
fsm_.Add(STATE::UNPERCHING_MOVING_TO_APPROACH_POSE,
MOTION_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::MOTION_FAILED;
err_msg_ = "Failed while moving to approach pose";
err_msg_ = "Failed while moving to approach pose. " + err_msg_;
Move(RECOVERY_POSE, ff_msgs::MotionGoal::NOMINAL);
return STATE::RECOVERY_MOVING_TO_RECOVERY_POSE;
});
// [32]
fsm_.Add(STATE::UNPERCHING_STOWING_ARM,
ARM_FAILED, [this](FSM::Event const& event) -> FSM::State {
err_ = RESPONSE::ARM_FAILED;
err_msg_ = "Failed while stowing the arm";
err_msg_ = "Failed while stowing the arm. " + err_msg_;
Arm(ff_msgs::ArmGoal::ARM_STOW);
return STATE::RECOVERY_STOWING_ARM;
});
Expand Down Expand Up @@ -750,6 +750,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet {
case ff_util::FreeFlyerActionState::SUCCESS:
return fsm_.Update(ARM_SUCCESS);
default:
err_msg_ = "Result code " + result->fsm_result;
return fsm_.Update(ARM_FAILED);
}
}
Expand Down Expand Up @@ -914,6 +915,7 @@ class PerchNodelet : public ff_util::FreeFlyerNodelet {
case ff_util::FreeFlyerActionState::SUCCESS:
return fsm_.Update(MOTION_SUCCESS);
default:
err_msg_ = "Result code " + result->fsm_result;
return fsm_.Update(MOTION_FAILED);
}
}
Expand Down

0 comments on commit 0d8b237

Please sign in to comment.