17 #include <boost/function_types/parameter_types.hpp> 18 #include <boost/typeof/std/utility.hpp> 20 #include <file_uploader_msgs/UploadFilesAction.h> 32 std::vector<std::string> & ros_bags_to_upload);
34 template<
typename RecorderFeedbackT,
typename RecorderStatusT>
38 RecorderFeedbackT& recorder_feedback,
39 RecorderStatusT& recording_status)
41 recorder_feedback.started = time_of_feedback;
42 recording_status.stage = stage;
43 recorder_feedback.status = recording_status;
46 template<
typename GoalHandleT,
typename UploadClientT>
48 GoalHandleT& goal_handle,
49 const double upload_time_out,
50 UploadClientT& upload_client,
51 std::vector<std::string>& ros_bags_to_upload)
53 AWS_LOG_INFO(__func__,
"Uploading Files.");
56 upload_client.sendGoal(goal);
60 using FuncType = decltype(&GoalHandleT::publishFeedback);
61 using FuncArgsType =
typename boost::function_types::parameter_types<FuncType>;
62 using ArgCrefType =
typename boost::mpl::at_c<FuncArgsType, 1>::type;
63 using ArgConstType =
typename boost::remove_reference<ArgCrefType>::type;
64 using ArgType =
typename boost::remove_const<ArgConstType>::type;
65 ArgType recorder_feedback;
66 recorder_msgs::RecorderStatus recording_status;
68 recorder_msgs::RecorderStatus::UPLOADING,
72 goal_handle.publishFeedback(recorder_feedback);
73 bool upload_finished =
true;
74 if (upload_time_out > 0) {
75 upload_finished = upload_client.waitForResult(
ros::Duration(upload_time_out));
77 upload_finished = upload_client.waitForResult();
80 return upload_finished;
83 template<
typename RecorderResultT>
84 void GenerateResult(uint8_t stage, std::string message, RecorderResultT& recorder_result)
86 recorder_result.result.result = stage;
87 recorder_result.result.message = std::move(message);
90 template<
typename GoalHandleT,
typename SimpleClientGoalStateT,
typename RecorderResultT>
92 GoalHandleT& goal_handle,
93 const SimpleClientGoalStateT& end_state,
95 RecorderResultT& recorder_result)
99 using FuncType = decltype(&GoalHandleT::publishFeedback);
100 using FuncArgsType =
typename boost::function_types::parameter_types<FuncType>;
101 using ArgCrefType =
typename boost::mpl::at_c<FuncArgsType, 1>::type;
102 using ArgConstType =
typename boost::remove_reference<ArgCrefType>::type;
103 using ArgType =
typename boost::remove_const<ArgConstType>::type;
104 ArgType recorder_feedback;
105 recorder_msgs::RecorderStatus recording_status;
107 recorder_msgs::RecorderStatus::COMPLETE,
111 goal_handle.publishFeedback(recorder_feedback);
114 if (!upload_finished) {
115 msg =
"File Uploader timed out.";
116 GenerateResult(recorder_msgs::RecorderResult::UPLOADER_TIMEOUT, msg, recorder_result);
117 goal_handle.setAborted(recorder_result, msg);
118 AWS_LOG_WARN(__func__, msg.c_str());
121 if (end_state != actionlib::SimpleClientGoalState::StateEnum::SUCCEEDED) {
122 msg =
"Upload failed with message: " + end_state.getText();
123 GenerateResult(recorder_msgs::RecorderResult::DEPENDENCY_FAILURE, msg, recorder_result);
124 goal_handle.setAborted(recorder_result, msg);
125 AWS_LOG_ERROR(__func__, msg.c_str());
127 msg =
"Upload Succeeded";
129 goal_handle.setSucceeded(recorder_result, msg);
130 AWS_LOG_INFO(__func__, msg.c_str());
file_uploader_msgs::UploadFilesGoal ConstructRosBagUploaderGoal(std::string destination, std::vector< std::string > &ros_bags_to_upload)
bool UploadFiles(GoalHandleT &goal_handle, const double upload_time_out, UploadClientT &upload_client, std::vector< std::string > &ros_bags_to_upload)
void GenerateResult(uint8_t stage, std::string message, RecorderResultT &recorder_result)
void HandleRecorderUploadResult(GoalHandleT &goal_handle, const SimpleClientGoalStateT &end_state, bool upload_finished, RecorderResultT &recorder_result)
void GenerateFeedback(uint8_t stage, ros::Time time_of_feedback, RecorderFeedbackT &recorder_feedback, RecorderStatusT &recording_status)