diff --git a/astrobee/behaviors/inspection/src/inspection_node.cc b/astrobee/behaviors/inspection/src/inspection_node.cc index 2a2f7d11..1897e8e1 100644 --- a/astrobee/behaviors/inspection/src/inspection_node.cc +++ b/astrobee/behaviors/inspection/src/inspection_node.cc @@ -414,7 +414,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet { } ROS_DEBUG_STREAM("Motion failed result error: " << result->response); - err_msg_ = "Result code " + result->response; + err_msg_ = "Move Code" + std::to_string(result->response) + ": (" + result->fsm_result + ")"; return fsm_.Update(MOTION_FAILED); } @@ -451,7 +451,7 @@ class InspectionNode : public ff_util::FreeFlyerNodelet { case ff_util::FreeFlyerActionState::SUCCESS: return fsm_.Update(DOCK_SUCCESS); default: - err_msg_ = "Result code " + result->response; + err_msg_ = "Dock Code " + std::to_string(result->response) + ": (" + result->fsm_result + ")"; return fsm_.Update(DOCK_FAILED); } } @@ -608,6 +608,9 @@ 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("sci_cam_max_trials")) { ROS_WARN_STREAM("Scicam didn't repond, resending it again"); diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index 0e70e781..223533f8 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -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(), ' ');