43 #include <boost/algorithm/string.hpp> 44 #include <boost/algorithm/string/split.hpp> 73 std::vector<std::string> options_strings;
76 std::vector<std::string> rov;
78 rov.push_back(
"rosrun");
79 rov.push_back(
"rosbag");
80 rov.push_back(
"record");
82 for (
auto it = options_strings.begin(); it != options_strings.end(); ++it) {
110 bool success =
false;
113 if (profile_name_to_record_options_map_.find(request.bag_profile_name) != profile_name_to_record_options_map_.end()) {
116 if (!profile_name_to_record_process_map_[request.bag_profile_name].getRecording()) {
118 pid_t pid_rr = fork();
121 std::vector<std::string> command_strings =
122 profile_name_to_record_process_map_[request.bag_profile_name].getRecordOptionsVector();
123 std::vector<char *> commands;
125 for (
auto it = command_strings.begin(); it != command_strings.end(); ++it) {
126 commands.push_back(const_cast<char *>(it->c_str()));
130 commands.push_back(NULL);
133 int dev_null_fd = open(
"/dev/null", O_WRONLY);
135 dup2(dev_null_fd, 1);
136 dup2(dev_null_fd, 2);
139 if (execvp(commands[0], &commands[0]) != 0) {
141 ROS_ERROR(
"execvp failed! %s", strerror(errno));
143 }
else if (pid_rr > 0) {
145 profile_name_to_record_process_map_[request.bag_profile_name].setRecordPID(pid_rr);
146 profile_name_to_record_process_map_[request.bag_profile_name].setRecording(
true);
147 ROS_INFO(
"successfully started the record. profile: %s", request.bag_profile_name.c_str());
159 ROS_WARN(
"record profile %s already recording", request.bag_profile_name.c_str());
164 if (profile_name_to_record_process_map_[request.bag_profile_name].getRecording()) {
165 if (kill(profile_name_to_record_process_map_[request.bag_profile_name].getRecordPID(), SIGINT) == 0) {
166 ROS_INFO(
"successfully stopped the record. profile: %s", request.bag_profile_name.c_str());
167 profile_name_to_record_process_map_[request.bag_profile_name].setRecording(
false);
170 ROS_ERROR(
"kill() when stopping record profile %s failed", request.bag_profile_name.c_str());
174 ROS_WARN(
"record profile %s already not recording", request.bag_profile_name.c_str());
179 ROS_ERROR(
"Requested record profile: %s does not exist. Check the configured " 181 request.bag_profile_name.c_str());
184 response.success = success;
187 publishBaggingStates();
193 bagger::BaggingState bs_msg;
201 for (
auto it = profile_name_to_record_process_map_.begin(); it != profile_name_to_record_process_map_.end(); ++it) {
202 bs_msg.bag_profile_names.push_back(it->first);
203 bs_msg.bagging.push_back(it->second.getRecording());
205 if (it->second.getRecording()) {
207 bs_msg.bag_file_names.push_back(getBagNameFromRecordOptions(profile_name_to_record_options_map_[it->first]));
209 bs_msg.bag_file_names.push_back(
"");
212 bagging_state_publisher_.publish(bs_msg);
216 std::string bag_name =
"";
217 std::string cwd_string = getCurrentWorkingDirectory();
219 if (record_opts.find(
"-o") != std::string::npos) {
225 size_t start_index = record_opts.find(
"-o ") + 3;
226 size_t next_space_index = record_opts.find(
" ", start_index);
227 std::string file_path = record_opts.substr(start_index, next_space_index - start_index);
230 if (file_path.find(
"/") == std::string::npos) {
231 file_path = cwd_string + file_path;
234 boost::filesystem::path full_file_name(file_path);
237 std::vector<boost::filesystem::path> matches =
238 getMatchingFilePathsInDirectory(full_file_name.parent_path(), full_file_name.stem().string());
240 if (matches.size() > 0) {
241 boost::filesystem::path latest = matches[0];
242 for (
auto i : matches) {
243 std::string
s = i.stem().string();
244 s.erase(std::remove_if(s.begin(), s.end(), std::not1(std::ptr_fun((
int (*)(
int)) std::isdigit))), s.end());
246 std::string latest_digits(latest.string());
247 latest_digits.erase(std::remove_if(latest_digits.begin(), latest_digits.end(),
248 std::not1(std::ptr_fun((
int (*)(
int)) std::isdigit))),
249 latest_digits.end());
250 if (s > latest_digits) {
254 bag_name = latest.string();
256 }
else if (record_opts.find(
"-O") != std::string::npos) {
262 size_t start_index = record_opts.find(
"-O ") + 3;
263 size_t next_space_index = record_opts.find(
" ", start_index);
264 std::string file_path = record_opts.substr(start_index, next_space_index - start_index);
267 if (file_path.find(
".bag") == std::string::npos) {
272 if (file_path.find(
"/") == std::string::npos) {
273 file_path = cwd_string + file_path;
275 bag_name = file_path;
280 const boost::filesystem::path cwd_path(cwd_string);
281 std::vector<boost::filesystem::path> bag_matches = getMatchingFilePathsInDirectory(cwd_path,
".bag.active");
283 if (bag_matches.size() > 0) {
284 boost::filesystem::path latest(
"");
286 for (
auto i : bag_matches) {
287 std::string
s = i.stem().string();
288 if (std::isdigit(s[0])) {
289 if (latest.string() ==
"") {
292 std::string latest_digits(latest.stem().string());
293 latest_digits.erase(std::remove_if(latest_digits.begin(), latest_digits.end(),
294 std::not1(std::ptr_fun((
int (*)(
int)) std::isdigit))),
295 latest_digits.end());
297 s.erase(std::remove_if(s.begin(), s.end(), std::not1(std::ptr_fun((
int (*)(
int)) std::isdigit))), s.end());
299 if (s > latest_digits) {
305 bag_name = latest.string();
310 if (record_opts.find(
"--split") != std::string::npos) {
311 auto last_underscore_index = bag_name.rfind(
'_');
312 if (last_underscore_index != std::string::npos) {
313 bag_name.erase(last_underscore_index);
317 bag_name = removeSuffix(bag_name,
".active");
318 return removeSuffix(bag_name,
".bag");
322 std::string match_string) {
323 std::vector<boost::filesystem::path> paths;
325 for (
auto &entry : boost::make_iterator_range(boost::filesystem::directory_iterator(dir_path), {})) {
326 if (entry.path().string().find(match_string) != std::string::npos) {
327 paths.push_back(entry.path());
335 if (s != suffix && s.size() > suffix.size() && s.substr(s.size() - suffix.size()) == suffix) {
336 s = s.substr(0, s.size() - suffix.size());
342 char path[FILENAME_MAX];
343 if (!getcwd(path,
sizeof(path))) {
346 return std::string(path) +
"/";
350 for (
auto &pn_pair : profile_name_to_record_process_map_) {
352 pid_t wait_result = waitpid(pn_pair.second.getRecordPID(), &status, WNOHANG);
353 if (wait_result > 0) {
354 ROS_DEBUG_STREAM(
"Record for profile " << pn_pair.first <<
" has completed");
356 pn_pair.second.setRecording(
false);
357 publishBaggingStates();
368 bagging_state_publisher_ = node_handle_->advertise<bagger::BaggingState>(
"bag_states", 1,
true);
373 if (private_node_handle_->hasParam(
"profile_name_to_record_options_map")) {
374 private_node_handle_->getParam(
"profile_name_to_record_options_map", profile_name_to_record_options_map_);
376 ROS_ERROR(
"Failed to find profile_name_to_bag_options_map in param server - " 378 profile_name_to_record_options_map_[
"everything"] =
"-a -O /tmp/everything.bag";
382 for (
auto it = profile_name_to_record_options_map_.begin(); it != profile_name_to_record_options_map_.end(); ++it) {
384 profile_name_to_record_process_map_[it->first] = rp;
387 publishBaggingStates();
389 int main(
int argc,
char **argv) {
int main(int argc, char **argv)
void attemptRecordCleanup()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
boost::shared_ptr< NodeHandle > NodeHandlePtr
std::vector< std::string > getRecordOptionsVector()
void setRecording(bool recording)
bool recording_
whether or not the rosbag record process is currently recording
std::string getName()
Getters.
std::string getBagNameFromRecordOptions(std::string record_opts)
Utility function which infers the most likely rosbag name for the passed record options.
#define ROS_DEBUG_STREAM(args)
Utility class for managing each spawned rosbag record process.
std::string removeSuffix(std::string s, std::string suffix)
Returns a version of the passed string with the passed suffix removed.
void publishBaggingStates()
Convenience function for publishing the recording states for each record profile. ...
bool onBagStateSrv(bagger::SetBagState::Request &request, bagger::SetBagState::Response &response)
Callback for receipt of set bag state service calls.
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
std::vector< boost::filesystem::path > getMatchingFilePathsInDirectory(const boost::filesystem::path &dir_path, std::string match_string)
void setRecordOptionString(std::string record_options)
ROSCPP_DECL void spinOnce()
void setName(std::string name)
pid_t record_pid_
pid of the spawned process - used to eventually terminate it
std::string getCurrentWorkingDirectory()
Utility function which returns the current working directory of the executing program.
std::string name_
name associated with the process - comes from the set record_profiles
std::string record_options_
record options associated with the process - comes from the set record_profiles
void setRecordPID(pid_t pid)
Setters.