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

Improve action result #163

Merged
merged 3 commits into from
Apr 29, 2024
Merged

Improve action result #163

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
c3371ee
passing on sub action results for cargo and inspection beh
marinagmoreira Apr 11, 2024
2da97fd
refining output, fixing bug, lab tested
marinagmoreira Apr 27, 2024
733009c
adding cargo feedback
marinagmoreira Apr 27, 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
8 changes: 6 additions & 2 deletions astrobee/behaviors/cargo/src/cargo_node.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -246,10 +246,10 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
[this](FSM::State const& state, FSM::Event const& event) -> FSM::State {
switch (event) {
case MOTION_FAILED:
Result(RESPONSE::MOTION_FAILED);
Result(RESPONSE::MOTION_FAILED, err_msg_);
break;
case ARM_FAILED:
Result(RESPONSE::ARM_FAILED);
Result(RESPONSE::ARM_FAILED, err_msg_);
break;
case DETECT_FAILED:
Result(RESPONSE::DETECT_FAILED);
Expand Down Expand Up @@ -542,6 +542,7 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
ROS_ERROR_STREAM("Invalid result received");
}

err_msg_ = "Move Code" + std::to_string(result->response) + ": (" + result->fsm_result + ")";
return fsm_.Update(MOTION_FAILED);
}

Expand All @@ -565,6 +566,7 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
case ff_util::FreeFlyerActionState::SUCCESS:
return fsm_.Update(ARM_SUCCESS);
default:
err_msg_ = "Arm Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")";
return fsm_.Update(ARM_FAILED);
}
}
Expand Down Expand Up @@ -835,6 +837,8 @@ class CargoNode : public ff_util::FreeFlyerNodelet {
isaac_msgs::CargoGoal goal_;
std::string cargo_id_;
isaac_msgs::CargoResult result_;
std::string err_msg_;

// Flag to wait for sci camera
bool sci_cam_req_ = false;
bool ground_active_ = false;
Expand Down
12 changes: 10 additions & 2 deletions astrobee/behaviors/inspection/src/inspection_node.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,7 +146,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
});
fsm_.Add(STATE::MOVING_TO_APPROACH_POSE,
DOCK_FAILED, [this](FSM::Event const& event) -> FSM::State {
Result(RESPONSE::DOCK_FAILED);
Result(RESPONSE::DOCK_FAILED, err_msg_);
return STATE::WAITING;
});
// [3]
Expand All @@ -165,7 +165,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
[this](FSM::State const& state, FSM::Event const& event) -> FSM::State {
switch (event) {
case MOTION_FAILED:
Result(RESPONSE::MOTION_APPROACH_FAILED);
Result(RESPONSE::MOTION_APPROACH_FAILED, err_msg_);
break;
case INSPECT_FAILED:
Result(RESPONSE::VISUAL_INSPECTION_FAILED);
Expand Down Expand Up @@ -414,6 +414,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
}
ROS_DEBUG_STREAM("Motion failed result error: " << result->response);

err_msg_ = "Move Code" + std::to_string(result->response) + ": (" + result->fsm_result + ")";
return fsm_.Update(MOTION_FAILED);
}

Expand Down Expand Up @@ -450,6 +451,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
case ff_util::FreeFlyerActionState::SUCCESS:
return fsm_.Update(DOCK_SUCCESS);
default:
err_msg_ = "Dock Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")";
return fsm_.Update(DOCK_FAILED);
}
}
Expand Down Expand Up @@ -606,11 +608,15 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
}
void SciCamTimeout(const ros::TimerEvent& event) {
sci_cam_timeout_.stop();
// If the action was cancelled stop taking more pictures
if (fsm_.GetState() == STATE::WAITING) return;

// The sci cam image was not received
if (sci_cam_req_ < cfg_.Get<int>("sci_cam_max_trials")) {
ROS_WARN_STREAM("Scicam didn't repond, resending it again");
// Send the command
SendPicture(focus_distance_current_);
++sci_cam_req_;
return;
} else {
return fsm_.Update(INSPECT_FAILED);
Expand Down Expand Up @@ -926,6 +932,8 @@ class InspectionNode : public ff_util::FreeFlyerNodelet {
std::string i_fsm_substate_;
isaac_msgs::InspectionResult result_;
int motion_retry_number_= 0;
std::string err_msg_;

// Flag to wait for sci camera
int sci_cam_req_ = 0;
bool ground_active_ = false;
Expand Down
2 changes: 1 addition & 1 deletion astrobee/behaviors/inspection/tools/inspection_tool.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -244,7 +244,7 @@ void ResultCallback(ff_util::FreeFlyerActionState::Enum code,
// Check that result var is valid
if (result != nullptr) {
// If we get there then we have some response data
s += result->fsm_result + " (Code " + std::to_string(result->response) + ")";
s += result->fsm_result + " (Inspection Code " + std::to_string(result->response) + ")";
}
// Limit line to the maximum amout of characters
if (s.size() < 71) s.append(71 - s.size(), ' ');
Expand Down
2 changes: 1 addition & 1 deletion isaac/config/behaviors/inspection.config
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -353,7 +353,7 @@ parameters = {
id = "sci_cam_max_trials",
reconfigurable = true,
type = "integer",
default = 3,
default = 10,
min = 0,
max = 10,
unit = "",
Expand Down
Loading