p36850791
/
kinetic-devel
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Restoring rosrecord so that rosrecord.py can be deprecated properly and downstream packages won't immediately have their dependencies broken

This commit is contained in:
Jeremy Leibs 2010-10-15 00:36:14 +00:00
parent 1ef78f763d
commit 224998103d
27 changed files with 3546 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
tools/rosbagmigration/src/rosbagmigration/__init__.py Normal file
View File

@ -0,0 +1 @@
from rosbag.migration import *

27
tools/rosrecord/CMakeLists.txt Normal file
View File

@ -0,0 +1,27 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rosbuild_init()
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
SET(CMAKE_SKIP_BUILD_RPATH FALSE)
SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
SET(CMAKE_INSTALL_RPATH "${PROJECT_SOURCE_DIR}/lib")
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
#rosbuild_add_executable(demo src/demo/demo.cpp)
rosbuild_add_library(rosrecorder src/rosrecord/recorder.cpp)
rosbuild_link_boost(rosrecorder iostreams system)
rosbuild_add_executable(rosrecord src/rosrecord/rosrecord.cpp)
target_link_libraries(rosrecord rosrecorder)
rosbuild_link_boost(rosrecord iostreams)
rosbuild_add_executable(rosplay src/rosplay/rosplay.cpp src/rosplay/time_publisher.cpp)
rosbuild_link_boost(rosplay iostreams system)
INSTALL(TARGETS rosrecord rosplay
RUNTIME DESTINATION $ENV{ROS_ROOT}/bin)
rosbuild_add_pyunit(test/test_rosrecord_offline.py)

53
tools/rosrecord/FORMATS Normal file
View File

@ -0,0 +1,53 @@
Version 0:
-------------
topic_name
md5sum
datatype
<repeating N>
time.sec
time.nsec
length
MSGDATA
-------------
Version 1.0:
-------------
#ROSLOG V1.0
quantity
<repeating quantity>
topic_name
md5sum
datatype
<repeating N>
topic_name
time.sec
time.nsec
length
MSGDATA
-------------
Version 1.1:
-------------
#ROSLOG V1.1
<repeating N>
topic_name
md5sum
datatype
time.sec
time.nsec
length
MSGDATA
-------------
Alternative Version 1.1:
-------------
#ROSRECORD V1.1
<repeating N>
topic_name
md5sum
datatype
time.sec
time.nsec
length
MSGDATA
-------------

47
tools/rosrecord/Makefile Normal file
View File

@ -0,0 +1,47 @@
# This was modified from the standard cmake.mk to perform and install step, and to do relinking as necessary on OS X
# set EXTRA_CMAKE_FLAGS in the including Makefile in order to add tweaks
CMAKE_FLAGS= -Wdev -DCMAKE_TOOLCHAIN_FILE=`rospack find rosbuild`/rostoolchain.cmake $(EXTRA_CMAKE_FLAGS)
# The all target does the heavy lifting, creating the build directory and
# invoking CMake
all:
@mkdir -p build
-mkdir -p bin
cd build && cmake $(CMAKE_FLAGS) ..
ifneq ($(MAKE),)
cd build && $(MAKE) $(ROS_PARALLEL_JOBS)
cd build && $(MAKE) install
else
cd build && make $(ROS_PARALLEL_JOBS)
cd build && make install
endif
@if test `uname` = Darwin ; then \
echo "Changing installed rosrecord and rosplay shared library location." && \
install_name_tool $(ROS_ROOT)/bin/rosrecord -change librosrecorder.dylib `rospack find rosrecord`/lib/librosrecorder.dylib && \
install_name_tool $(ROS_ROOT)/bin/rosplay -change librosrecorder.dylib `rospack find rosrecord`/lib/librosrecorder.dylib; \
fi
PACKAGE_NAME=$(shell basename $(PWD))
# The clean target blows everything away
# It also removes auto-generated message/service code directories,
# to handle the case where the original .msg/.srv file has been removed,
# and thus CMake no longer knows about it.
clean:
-cd build && make clean
rm -rf msg/cpp msg/lisp msg/oct msg/java srv/cpp srv/lisp srv/oct srv/java src/$(PACKAGE_NAME)/msg src/$(PACKAGE_NAME)/srv
rm -rf build
rm -f .build-version
# All other targets are just passed through
test: all
if cd build && make -k $@; then make test-results; else make test-results && exit 1; fi
tests: all
cd build && make $@
test-future: all
cd build && make -k $@
gcoverage: all
cd build && make $@
include $(shell rospack find mk)/buildtest.mk

55
tools/rosrecord/include/rosrecord/AnyMsg.h Normal file
View File

@ -0,0 +1,55 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ANYMSG_H
#define ANYMSG_H
#include "ros/message.h"
class AnyMsg : public ros::Message
{
public:
inline static std::string __s_getDataType() { return std::string("*"); }
inline static std::string __s_getMD5Sum() { return std::string("*"); }
inline static std::string __s_getMessageDefinition() { return std::string(""); }
virtual const std::string __getDataType() const { return __s_getDataType(); }
virtual const std::string __getMD5Sum() const { return __s_getMD5Sum(); }
virtual const std::string __getMessageDefinition() const { return __s_getMessageDefinition(); }
virtual uint32_t serializationLength() const { return 0; }
virtual uint8_t *serialize(uint8_t *write_ptr, uint32_t) const { assert(0); return NULL; }
virtual uint8_t *deserialize(uint8_t *read_ptr) { assert(0); return NULL; }
};
#endif

130
tools/rosrecord/include/rosrecord/MsgFunctor.h Normal file
View File

@ -0,0 +1,130 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef MSGFUNCTOR_H
#define MSGFUNCTOR_H
#include "ros/time.h"
#include <string>
#include "std_msgs/String.h"
#include "topic_tools/shape_shifter.h"
class AbstractMsgFunctor
{
public:
virtual void call(std::string, ros::Message*, ros::Time, ros::Time) = 0;
virtual ros::Message* allocateMsg() = 0;
virtual ~AbstractMsgFunctor() { }
};
class EmptyObject { };
template<class M, class T = EmptyObject>
class MsgFunctor : public AbstractMsgFunctor
{
public:
MsgFunctor(void(*fp)(std::string, M*, ros::Time, ros::Time, void*), void* user_data) :
inflate_(true), obj_(NULL), fp_(NULL), fp_typed_(fp), fp_obj_(NULL), fp_typed_obj_(NULL), user_data_(user_data) { }
MsgFunctor(void(*fp)(std::string, ros::Message*, ros::Time, ros::Time, void*), void* user_data, bool inflate) :
inflate_(inflate), obj_(NULL), fp_(fp), fp_typed_(NULL), fp_obj_(NULL), fp_typed_obj_(NULL), user_data_(user_data) {
}
MsgFunctor(T* obj, void(T::*fp)(std::string, M*, ros::Time, ros::Time, void*), void* user_data) :
inflate_(true), obj_(obj), fp_(NULL), fp_typed_(NULL), fp_obj_(NULL), fp_typed_obj_(fp), user_data_(user_data) {
}
MsgFunctor(T* obj, void(T::*fp)(std::string, ros::Message*, ros::Time, ros::Time, void*), void* user_data, bool inflate) :
inflate_(inflate), obj_(obj), fp_(NULL), fp_typed_(NULL), fp_obj_(fp), fp_typed_obj_(NULL), user_data_(user_data) {
}
virtual void call(std::string topic_name, ros::Message* m, ros::Time time_play, ros::Time time_recorded) {
if (M::__s_getDataType() == std::string("*"))
{
// Handle AnyMsg
if (obj_) {
if (fp_obj_)
(*obj_.*fp_obj_)(topic_name, m, time_play, time_recorded, user_data_);
else if (fp_typed_obj_)
assert(0);
}
else {
if (fp_)
(*fp_)(topic_name, m, time_play, time_recorded, user_data_);
else if (fp_typed_)
assert(0);
}
}
else
{
topic_tools::ShapeShifter* ss = (topic_tools::ShapeShifter*) m;
boost::shared_ptr<M> msg = ss->instantiate<M>();
ros::assignSubscriptionConnectionHeader(msg.get(), ss->__connection_header);
if (obj_) {
if (fp_obj_)
(*obj_.*fp_obj_)(topic_name, (ros::Message*) msg.get(), time_play, time_recorded, user_data_);
else if (fp_typed_obj_)
(*obj_.*fp_typed_obj_)(topic_name, (M*) msg.get(), time_play, time_recorded, user_data_);
}
else {
if (fp_)
(*fp_)(topic_name, (ros::Message*) msg.get(), time_play, time_recorded, user_data_);
else if (fp_typed_)
(*fp_typed_)(topic_name, (M*) msg.get(), time_play, time_recorded, user_data_);
}
}
}
virtual ros::Message* allocateMsg() {
if (fp_typed_ || fp_typed_obj_)
return new M;
else
return NULL;
}
private:
bool inflate_;
T* obj_;
void (*fp_)(std::string, ros::Message*, ros::Time, ros::Time, void*);
void (*fp_typed_)(std::string, M*, ros::Time, ros::Time, void*);
void (T::*fp_obj_)(std::string, ros::Message*, ros::Time, ros::Time, void*);
void (T::*fp_typed_obj_)(std::string, M*, ros::Time, ros::Time, void*);
void *user_data_;
};
#endif

401
tools/rosrecord/include/rosrecord/Player.h Normal file
View File

@ -0,0 +1,401 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef IGNORE_ROSRECORD_DEPRECATED
#warning rosrecord/Player.h has been deprecated
#endif
#ifndef ROSRECORDPLAYER_H
#define ROSRECORDPLAYER_H
#include "rosrecord/AnyMsg.h"
#include "rosrecord/MsgFunctor.h"
#include "rosrecord/constants.h"
#include "rosbag/bag.h"
#include "rosbag/view.h"
#include "rosbag/time_translator.h"
#include "topic_tools/shape_shifter.h"
#include "std_msgs/String.h"
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
namespace ros
{
namespace record
{
class Player
{
struct FilteredMsgFunctor
{
std::string topic_name;
std::string md5sum;
std::string datatype;
bool inflate;
AbstractMsgFunctor* f;
};
public:
ROSCPP_DEPRECATED Player(double time_scale=1.0) //time_scale_(time_scale)
{
translator_.setTimeScale(time_scale);
}
virtual ~Player() {
foreach(FilteredMsgFunctor const& callback, callbacks_)
delete callback.f;
close();
}
std::string getVersionString() {
std::stringstream ss;
ss << bag_.getMajorVersion() << "." << bag_.getMinorVersion();
return ss.str();
}
bool isDone() { return iterator_ == view_.end(); }
// todo: implement
ros::Duration getFirstDuration() { return first_duration_; }
ros::Time getFirstTime() { return first_time_; }
void setTranslatedStartTime(const ros::Time& start_time)
{
translator_.setTranslatedStartTime(start_time);
}
void setRealStartTime(const ros::Time& start_time)
{
translator_.setRealStartTime(start_time);
}
// todo: implement
ros::Duration get_duration() { return duration_; }
void close() {
iterator_ = view_.end();
bag_.close();
}
bool open(std::string const& file_name, ros::Time start_time, bool try_future = false) {
translator_.setTranslatedStartTime(start_time);
std::string ext = boost::filesystem::extension(file_name);
if (ext != ".bag") {
ROS_ERROR("File: '%s' does not have .bag extension",file_name.c_str());
return false;
}
try
{
bag_.open(file_name, rosbag::bagmode::Read);
}
catch (rosbag::BagException ex)
{
ROS_FATAL_STREAM("Failed to open file: " << file_name << " (" << ex.what() << ")");
return false;
}
view_.addQuery(bag_);
iterator_ = view_.begin();
if (iterator_ != view_.end())
{
translator_.setRealStartTime(iterator_->getTime());
first_time_ = iterator_->getTime();
first_duration_ = iterator_->getTime() - ros::Time(0,0);
duration_ = iterator_->getTime() - first_time_;
}
return true;
}
template<class M>
void addHandler(std::string topic_name, void(*fp)(std::string, ros::Message*, ros::Time, ros::Time, void*), void* ptr, bool inflate)
{
FilteredMsgFunctor fmf;
fmf.topic_name = topic_name;
fmf.md5sum = M::__s_getMD5Sum();
fmf.datatype = M::__s_getDataType();
fmf.inflate = inflate;
fmf.f = new MsgFunctor<M>(fp, ptr, inflate);
callbacks_.push_back(fmf);
}
// Handler for explicit message type MUST be inflated (Note function pointer takes M vs. ros::Message)
template<class M>
void addHandler(std::string topic_name, void(*fp)(std::string, M*, ros::Time, ros::Time, void*), void* ptr)
{
FilteredMsgFunctor fmf;
fmf.topic_name = topic_name;
fmf.md5sum = M::__s_getMD5Sum();
fmf.datatype = M::__s_getDataType();
fmf.inflate = true;
fmf.f = new MsgFunctor<M>(fp, ptr);
callbacks_.push_back(fmf);
}
template<class M, class T>
void addHandler(std::string topic_name, void(T::*fp)(std::string, ros::Message*, ros::Time, ros::Time, void*), T* obj, void* ptr, bool inflate)
{
FilteredMsgFunctor fmf;
fmf.topic_name = topic_name;
fmf.md5sum = M::__s_getMD5Sum();
fmf.datatype = M::__s_getDataType();
fmf.inflate = inflate;
fmf.f = new MsgFunctor<M, T>(obj, fp, ptr, inflate);
callbacks_.push_back(fmf);
}
// Handler for explicit message type MUST be inflated (Note function pointer takes M vs. ros::Message)
template<class M, class T>
void addHandler(std::string topic_name, void(T::*fp)(std::string, M*, ros::Time, ros::Time, void*), T* obj, void* ptr)
{
FilteredMsgFunctor fmf;
fmf.topic_name = topic_name;
fmf.md5sum = M::__s_getMD5Sum();
fmf.datatype = M::__s_getDataType();
fmf.inflate = true;
fmf.f = new MsgFunctor<M, T>(obj, fp, ptr);
callbacks_.push_back(fmf);
}
ros::Time get_next_msg_time() {
if (isDone())
return ros::Time();
return (*iterator_).getTime();
}
bool nextMsg() {
if (iterator_ == view_.end())
return false;
rosbag::MessageInstance msg = (*iterator_);
std::string const& topic = msg.getTopic();
std::string const& md5sum = msg.getMD5Sum();
std::string const& datatype = msg.getDataType();
duration_ = msg.getTime() - first_time_;
// Filter the list of callbacks
std::vector<FilteredMsgFunctor> callbacks;
foreach(FilteredMsgFunctor fmf, callbacks_)
{
if (topic != fmf.topic_name && fmf.topic_name != std::string("*"))
continue;
if (fmf.md5sum != md5sum && fmf.md5sum != std::string("*"))
continue;
if (fmf.datatype != datatype && fmf.datatype != std::string("*") && datatype != std::string("*"))
continue;
callbacks.push_back(fmf);
}
if (callbacks.size() > 0) {
boost::shared_ptr<topic_tools::ShapeShifter const> ss = msg.instantiate<topic_tools::ShapeShifter>();
ros::Time const& time = msg.getTime();
ros::Time const& translated_time = translator_.translate(time);
foreach(FilteredMsgFunctor fmf, callbacks)
fmf.f->call(topic, (ros::Message*) ss.get(), translated_time, time);
}
iterator_++;
return true;
}
void shiftTime(ros::Duration shift) {
translator_.shift(shift);
}
private:
//double time_scale_;
//ros::Time start_time_;
rosbag::TimeTranslator translator_;
ros::Time first_time_;
ros::Duration first_duration_;
ros::Duration duration_;
rosbag::Bag bag_;
rosbag::View view_;
rosbag::View::const_iterator iterator_;
std::vector<FilteredMsgFunctor> callbacks_;
};
class MultiPlayer
{
public:
MultiPlayer() { }
~MultiPlayer() {
foreach(Player* player, players_)
delete player;
}
ros::Duration getDuration()
{
ros::Duration d(0.0);
foreach(Player* player, players_) {
ros::Duration dd = player->get_duration();
if (dd > d)
d = dd;
}
return d;
}
ros::Time getFirstTime() { return first_time_; }
bool open(std::vector<std::string> file_names, ros::Time start, double time_scale = 1, bool try_future = false)
{
ros::Duration first_duration;
foreach(std::string file_name, file_names) {
Player* l = new Player(time_scale);
if (l->open(file_name, ros::Time(), try_future)) {
players_.push_back(l);
// We don't actually use first_duration
if (first_duration == ros::Duration() || l->getFirstDuration() < first_duration)
first_duration = l->getFirstDuration();
if (first_time_ == ros::Time() || l->getFirstTime() < first_time_)
first_time_ = l->getFirstTime();
}
else {
delete l;
return false;
}
}
foreach(Player* player, players_)
{
player->setRealStartTime(first_time_);
player->setTranslatedStartTime(start);
}
return true;
}
void shiftTime(ros::Duration shift)
{
for (std::vector<Player*>::iterator player_it = players_.begin(); player_it != players_.end(); player_it++)
(*player_it)->shiftTime(shift);
}
template<class M>
void addHandler(std::string topic_name, void(*fp)(std::string, ros::Message*, ros::Time, ros::Time, void*), void* ptr, bool inflate)
{
for (std::vector<Player*>::iterator player_it = players_.begin(); player_it != players_.end(); player_it++)
(*player_it)->addHandler<M>(topic_name, fp, ptr, inflate);
}
template<class M>
void addHandler(std::string topic_name, void(*fp)(std::string, M*, ros::Time, ros::Time, void*), void* ptr)
{
for (std::vector<Player*>::iterator player_it = players_.begin(); player_it != players_.end(); player_it++)
(*player_it)->addHandler<M>(topic_name, fp, ptr);
}
template<class M, class T>
void addHandler(std::string topic_name, void(T::*fp)(std::string, ros::Message*, ros::Time, ros::Time, void*), T* obj, void* ptr, bool inflate)
{
for (std::vector<Player*>::iterator player_it = players_.begin(); player_it != players_.end(); player_it++)
(*player_it)->addHandler<M>(topic_name, fp, obj, ptr, inflate);
}
template<class M, class T>
void addHandler(std::string topic_name, void(T::*fp)(std::string, M*, ros::Time, ros::Time, void*), T* obj, void* ptr)
{
for (std::vector<Player*>::iterator player_it = players_.begin(); player_it != players_.end(); player_it++)
(*player_it)->addHandler<M>(topic_name, fp, obj, ptr);
}
bool nextMsg()
{
Player* next_player = NULL;
bool first = true;
ros::Time min_t = ros::TIME_MAX;
bool remaining = false;
foreach(Player* player, players_) {
if (player->isDone())
continue;
remaining = true;
ros::Time t = player->get_next_msg_time();
if (first || t < min_t) {
first = false;
next_player = player;
min_t = player->get_next_msg_time();
}
}
if (next_player)
next_player->nextMsg();
return remaining;
}
private:
std::vector<Player*> players_;
ros::Time first_time_;
};
}
}
#endif

91
tools/rosrecord/include/rosrecord/Recorder.h Normal file
View File

@ -0,0 +1,91 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef IGNORE_ROSRECORD_DEPRECATED
#warning rosrecord/Recorder.h has been deprecated
#endif
#ifndef ROSRECORDRECORDER_H
#define ROSRECORDRECORDER_H
#include "ros/header.h"
#include "ros/time.h"
#include "ros/message.h"
#include <boost/thread/mutex.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <fstream>
#include <iostream>
#include <map>
#include "rosbag/bag.h"
namespace ros
{
namespace record
{
typedef unsigned long long pos_t;
class Recorder
{
public:
ROSCPP_DEPRECATED Recorder();
virtual ~Recorder();
bool open(const std::string& file_name, bool random_access=false);
bool record(std::string topic_name, ros::Message::ConstPtr msg, ros::Time time);
bool record(std::string topic_name, const ros::Message& msg, ros::Time time);
void close();
pos_t getOffset();
private:
bool checkDisk();
private:
rosbag::Bag bag_;
bool logging_enabled_;
boost::mutex check_disk_mutex_;
ros::WallTime check_disk_next_;
ros::WallTime warn_next_;
};
}
}
#endif

81
tools/rosrecord/include/rosrecord/constants.h Normal file
View File

@ -0,0 +1,81 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef ROSRECORD_CONSTANTS_H
#define ROSRECORD_CONSTANTS_H
#include <string>
#include <stdint.h>
namespace ros
{
namespace record
{
// Log file version to write
static const std::string VERSION = "1.2";
// Bytes reserved for file header record (4KB)
static const uint32_t FILE_HEADER_LENGTH = 4 * 1024;
// Header field delimiter
static const unsigned char FIELD_DELIM = '=';
// Header field keys
static const std::string OP_FIELD_NAME = "op";
static const std::string TOPIC_FIELD_NAME = "topic";
static const std::string MD5_FIELD_NAME = "md5";
static const std::string SEC_FIELD_NAME = "sec";
static const std::string NSEC_FIELD_NAME = "nsec";
static const std::string TYPE_FIELD_NAME = "type";
static const std::string DEF_FIELD_NAME = "def";
static const std::string LATCHING_FIELD_NAME = "latching";
static const std::string CALLERID_FIELD_NAME = "callerid";
static const std::string VER_FIELD_NAME = "ver";
static const std::string COUNT_FIELD_NAME = "count";
static const std::string INDEX_POS_FIELD_NAME = "index_pos";
// Index data record version to write
static const uint32_t INDEX_VERSION = 0;
// The valid values of the "op" field
static const unsigned char OP_MSG_DEF = 0x01;
static const unsigned char OP_MSG_DATA = 0x02;
static const unsigned char OP_FILE_HEADER = 0x03;
static const unsigned char OP_INDEX_DATA = 0x04;
}
}
#endif

91
tools/rosrecord/include/rosrecord/rosplay.h Normal file
View File

@ -0,0 +1,91 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#ifndef ROSPLAY_H
#define ROSPLAY_H
#include <time.h>
#include <sys/stat.h>
#include "ros/ros.h"
#include "ros/time.h"
#include <string>
#include "rosrecord/AnyMsg.h"
#include "rosrecord/time_publisher.h"
#include <unistd.h>
#include <termios.h>
// We know this is deprecated..
#define IGNORE_ROSRECORD_DEPRECATED
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
#include "rosrecord/Player.h"
class RosPlay
{
public:
RosPlay(int i_argc, char **i_argv);
~RosPlay();
bool spin();
private:
ros::Time getSysTime();
void doPublish(std::string name, ros::Message* m, ros::Time play_time, ros::Time record_time, void* n);
// This is a pointer to allow player to start before node handle
// exists since this is where argument parsing happens.
ros::NodeHandle* node_handle;
bool bag_time_initialized_, at_once_, quiet_, paused_, shifted_, bag_time_;
double time_scale_;
ros::WallTime start_time_, requested_start_time_, paused_time_;
ros::record::MultiPlayer player_;
int queue_size_;
ros::WallDuration advertise_sleep_;
termios orig_flags_;
// This internall containers a nodehandle so we make a pointer for the same reason
SimpleTimePublisher* bag_time_publisher_;
double bag_time_frequency_;
fd_set stdin_fdset_;
int maxfd_;
};
#endif

144
tools/rosrecord/include/rosrecord/time_publisher.h Normal file
View File

@ -0,0 +1,144 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef TIME_PUBLISHER_H
#define TIME_PUBLISHER_H
#include "ros/ros.h"
#include "ros/time.h"
#include <boost/thread/thread.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/thread/condition.hpp>
#include <boost/bind.hpp>
#include <iostream>
#include "sys/time.h"
class SimpleTimePublisher {
public:
/*! Create a time publisher
* A publish_frequency of < 0 indicates that time shouldn't actually be published
*/
SimpleTimePublisher(double publish_frequency, double time_scale);
/*! Set the horizon that the clock will run to */
void setHorizon(const ros::Time& horizon);
/*! Set the horizon that the clock will run to */
void setWCHorizon(const ros::WallTime& horizon);
/*! Set the current time */
void setTime(const ros::Time& time);
/*! Run the clock for AT MOST duration
*
* If horizon has been reached this function returns immediately
*/
void runClock(const ros::WallDuration& duration);
//! Sleep as necessary, but don't let the click run
void runStalledClock(const ros::WallDuration& duration);
//! Step the clock to the horizon
void stepClock();
bool horizonReached();
private:
bool do_publish_;
double publish_frequency_;
double time_scale_;
ros::NodeHandle node_handle_;
ros::Publisher time_pub_;
ros::WallDuration wall_step_;
ros::WallTime next_pub_;
ros::WallTime wc_horizon_;
ros::Time horizon_;
ros::Time current_;
};
class TimePublisher {
public:
TimePublisher();
~TimePublisher();
// Initialize publisher
void initialize(double publish_frequency, double time_scale_factor = 1.0);
// Freeze time by publishing the repeatedly publishing the same time.
void freezeTime();
// start time at bag timepoint bag_time
void startTime(ros::Time bag_time);
// step the timepoint bag_time
void stepTime(ros::Time bag_time);
// Update the time up to which the publisher is allowed to run
void setHorizon(ros::Time& horizon);
private:
// get the system time
ros::Time getSysTime();
// publish time
void publishTime();
// Accessor, with lock
const ros::Time& getHorizon();
double publish_freq_;
ros::NodeHandle node_handle;
ros::Publisher time_pub;
ros::Time horizon_; // The time that we're allowed to publish until
ros::Time last_pub_time_; // A time in the bag's time.
ros::Time last_sys_time_; // The start/restart time at playback
bool freeze_time_; // Stop time.
bool is_started_;
boost::mutex offset_mutex_;
boost::thread *publish_thread_;
double time_scale_factor_;
volatile bool continue_;
};
#endif

9
tools/rosrecord/mainpage.dox Normal file
View File

@ -0,0 +1,9 @@
/**
\mainpage
\htmlinclude manifest.html
\b rosrecord contains a collection of command-line tools for recording messages to \b bag files
and playing them back. ros::record::Player provides a C++ interface for reading from these bag files.
*/

29
tools/rosrecord/manifest.xml Normal file
View File

@ -0,0 +1,29 @@
<package>
<description brief="ROS Bag Tools">
Rosrecord has been deprecated as of ROS-1.1.5. Please transition over
to use the rosbag package instead.
This is a set of tools for recording from and playing back to ros
topics. It is intended to be high performance and avoids
deserialization and reserialization of the messages.
</description>
<author>Jeremy Leibs (leibs@willowgarage.com), python code by James Bowman (jamesb@willowgarage.com) and Ken Conley (kwc@willowgarage.com)</author>
<license>BSD</license>
<review status="Doc reviewed" notes="2010/01/20"/>
<url>http://ros.org/wiki/rosrecord</url>
<depend package="roscpp"/>
<depend package="rospy"/>
<depend package="topic_tools"/>
<depend package="rostest"/>
<depend package="rosbag"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lrosrecorder"/>
<rosdoc config="${prefix}/rosdoc.yaml" />
</export>
<rosdep name="python-imaging"/>
<platform os="ubuntu" version="9.04"/>
<platform os="ubuntu" version="9.10"/>
<platform os="ubuntu" version="10.04"/>
<platform os="macports" version="macports"/>
</package>

6
tools/rosrecord/rosdoc.yaml Normal file
View File

@ -0,0 +1,6 @@
- builder: epydoc
output_dir: python
- builder: doxygen
name: C++ API
output_dir: c++
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

73
tools/rosrecord/scripts/bag2png.py Executable file
View File

@ -0,0 +1,73 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
import sys
import array
import Image
def int16_str(d):
return array.array('B', [ min(x,255) for x in d ]).tostring()
#return array.array('f', [ float(x) for x in d ]).tostring()
def msg2im(msg):
""" Take an image_msgs::Image and return a PIL image """
if len(msg.uint8_data.data) == 0 and len(msg.int16_data.data) == 0:
return None
else:
if msg.depth == 'uint8':
ma = msg.uint8_data
image_data = ma.data
else:
ma = msg.int16_data
image_data = int16_str(ma.data)
dim = dict([ (d.label,d.size) for d in ma.layout.dim ])
mode = { ('uint8',1) : "L", ('uint8',3) : "RGB", ('int16',1) : "L" }[msg.depth, dim['channel']]
(w,h) = (dim['width'], dim['height'])
return Image.fromstring(mode, (w,h), image_data)
counter = 0
for topic, msg, t in rosrecord.logplayer(sys.argv[1]):
if rospy.is_shutdown():
break
if topic.endswith("stereo/raw_stereo"):
for (mi,c) in [ (msg.left_image, 'L'), (msg.right_image, 'R'), (msg.disparity_image, 'D')]:
im = msg2im(mi)
if im:
ext = { 'L':'png', 'RGB':'png', 'F':'tiff' }[im.mode]
im.save("%06d%s.%s" % (counter, c, ext))
counter += 1

70
tools/rosrecord/scripts/bagsort.py Executable file
View File

@ -0,0 +1,70 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
def sortbags(inbag, outbag):
rebag = rosrecord.Rebagger(outbag)
schedule = []
for i, (topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)):
if rospy.is_shutdown():
break
schedule.append((t, i))
schedule = [ i for (t,i) in sorted(schedule) ]
print schedule
stage = {}
for i, (topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)):
if rospy.is_shutdown():
break
stage[i] = (topic, msg, t)
while (schedule != []) and (schedule[0] in stage):
(topic, msg, t) = stage[schedule[0]]
rebag.add(topic, msg, t, raw=True)
del stage[schedule[0]]
schedule = schedule[1:]
assert schedule == []
assert stage == {}
rebag.close()
if __name__ == '__main__':
import sys
if len(sys.argv) == 3:
sortbags(sys.argv[1], sys.argv[2])
else:
print "usage: bagsort <inbag> <outbag>"

22
tools/rosrecord/scripts/fastrebag.py Executable file
View File

@ -0,0 +1,22 @@
#!/usr/bin/env python
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
import fileinput
def fastrebag(inbag, outbag):
rebag = rosrecord.Rebagger(outbag)
for i,(topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)):
rebag.add(topic, msg, t, raw=True)
rebag.close()
if __name__ == '__main__':
import sys
if len(sys.argv) == 3:
fastrebag(sys.argv[1], sys.argv[2])
else:
print "usage: fastrebag.py <inbag> <outbag>"

39
tools/rosrecord/scripts/fix_md5sums.py Executable file
View File

@ -0,0 +1,39 @@
#!/usr/bin/env python
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
import fileinput
import os
def fix_md5sums(inbags):
for b in inbags:
print "Trying to migrating file: %s"%b
outbag = b+'.tmp'
rebag = rosrecord.Rebagger(outbag)
try:
for i,(topic, msg, t) in enumerate(rosrecord.logplayer(b, raw=True)):
rebag.add(topic, msg, t, raw=True)
rebag.close()
except rosrecord.ROSRecordException, e:
print " Migration failed: %s"%(e.message)
os.remove(outbag)
continue
oldnamebase = b+'.old'
oldname = oldnamebase
i = 1
while os.path.isfile(oldname):
i=i+1
oldname = oldnamebase + str(i)
os.rename(b, oldname)
os.rename(outbag, b)
print " Migration successful. Original stored as: %s"%oldname
if __name__ == '__main__':
import sys
if len(sys.argv) >= 2:
fix_md5sums(sys.argv[1:])
else:
print "usage: fix_md5sums.py bag1 [bag2 bag3 ...]"

41
tools/rosrecord/scripts/fix_moved_messages.py Executable file
View File

@ -0,0 +1,41 @@
#!/usr/bin/env python
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
import fileinput
def fixbags(md5file, inbag, outbag):
d = dict()
finput = fileinput.input(md5file)
for line in finput:
sp = line.split()
d[sp[1]] = [sp[0], sp[2], sp[3]]
rebag = rosrecord.Rebagger(outbag)
for i,(topic, msg, t) in enumerate(rosrecord.logplayer(inbag, raw=True)):
if rospy.is_shutdown():
break
type = msg[0]
bytes = msg[1]
md5 = msg[2]
if md5 in d:
if type != d[md5][0]:
print 'WARNING: found matching md5, but non-matching name'
continue
msg = (d[md5][1], msg[1], d[md5][2])
rebag.add(topic, msg, t, raw=True)
rebag.close()
if __name__ == '__main__':
import sys
if len(sys.argv) == 4:
fixbags(sys.argv[1], sys.argv[2], sys.argv[3])
else:
print "usage: fix_moved_messages.py <name_md5_file> <inbag> <outbag>"

57
tools/rosrecord/scripts/topic_renamer.py Normal file
View File

@ -0,0 +1,57 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import rospy
import rosrecord
def rename_topic(intopic, inbag, outtopic, outbag):
rebag = rosrecord.Rebagger(outbag)
for (topic, msg, t) in rosrecord.logplayer(inbag, raw = True):
if rospy.is_shutdown():
break
if topic == intopic:
rebag.add(outtopic, msg, t, raw=True)
else:
rebag.add(topic, msg, t, raw=True)
rebag.close()
if __name__ == '__main__':
import sys
if len(sys.argv) == 5:
rename_topic(sys.argv[1], sys.argv[2], sys.argv[3], sys.argv[4])
else:
print "usage: topic_renamer <intopic> <inbag> <outtopic> <outbag>"

101
tools/rosrecord/src/demo/demo.cpp Normal file
View File

@ -0,0 +1,101 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "ros/node.h"
#include <string>
#include <map>
#include "rosrecord/Player.h"
#include "rosrecord/AnyMsg.h"
#include "std_msgs/String.h"
using namespace std;
void all_handler(string name, ros::Message* m, ros::Time t, ros::Time t_no_use, void* n)
{
int* counter = (int*)(n);
cout << "all_handler saw a message named " << name << " with datatype: " << m->__getDataType() << " Count at: " << (*counter)++ << endl;
}
void string_handler(string name, std_msgs::String* str, ros::Time t, ros::Time t_no_use, void* n)
{
cout << "string_handler saw a message named " << name << " with datatype: " << str->__getDataType() << " and value: " << str->data << endl;
}
class Handler
{
public:
int counter;
Handler() : counter(0) {}
void all_handler(string name, ros::Message* m, ros::Time t, ros::Time t_no_use, void* n)
{
cout << "Handler::all_handler saw a message named " << name << " with datatype: " << m->__getDataType() << " Count at: " << counter++ << endl;
}
void string_handler(string name, std_msgs::String* str, ros::Time t, ros::Time t_no_use, void* n)
{
cout << "Handler::string_handler saw a message named " << name << " with datatype: " << str->__getDataType() << " and value: " << str->data << endl;
}
};
int main(int argc, char **argv)
{
if (argc != 2)
{
cout << endl << "usage:" << endl << " demo BAG" << endl;
return 1;
}
ros::record::Player player;
Handler handler;
int counter = 0;
if (player.open(string(argv[1]), ros::Time()))
{
player.addHandler<AnyMsg>(string("*"), &all_handler, &counter, false);
player.addHandler<std_msgs::String>(string("chatter"), &string_handler, NULL);
player.addHandler<AnyMsg>(string("babble"), &Handler::all_handler, &handler, &counter, false);
player.addHandler<std_msgs::String>(string("*"), &Handler::string_handler, &handler, NULL);
}
while(player.nextMsg())
{
}
return 0;
}

514
tools/rosrecord/src/rosplay/rosplay.cpp Normal file
View File

@ -0,0 +1,514 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosrecord/rosplay.h"
#include "sys/select.h"
using namespace std;
using namespace ros;
void print_usage()
{
fprintf (stderr, "Usage: rosplay [options] BAG1 [BAG2]\n");
}
void print_help()
{
print_usage();
fprintf(stderr, " -n\tdisable display of current log time\n");
fprintf(stderr, " -c\tcheck the contents of the bag without playing back\n");
fprintf(stderr, " -a\tplayback all messages without waiting\n");
fprintf(stderr, " -b hz\tpublish the bag time at frequence <hz>\n");
fprintf(stderr, " -p\tstart in paused mode\n");
fprintf(stderr, " -r\tincrease the publish rate ba a factor <rate_change>\n");
fprintf(stderr, " -s sec\tsleep <sec> sleep duration after every advertise call (to allow subscribers to connect)\n");
fprintf(stderr, " -t sec\tstart <sec> seconds into the files\n");
fprintf(stderr, " -q sz\tUse an outgoing queue of size <sz> (defaults to 0)\n");
fprintf(stderr, " -T\tTry to play future version.\n");
fprintf(stderr, " -h\tdisplay this help message\n");
}
RosPlay::RosPlay(int i_argc, char **i_argv) :
node_handle(NULL),
bag_time_initialized_(false),
at_once_(false),
quiet_(false),
paused_(false),
bag_time_(false),
time_scale_(1.0),
queue_size_(0),
bag_time_publisher_(NULL)
{
const int fd = fileno(stdin);
advertise_sleep_ = ros::WallDuration(.2);
termios flags;
tcgetattr(fd,&orig_flags_);
flags = orig_flags_;
flags.c_lflag &= ~ICANON; // set raw (unset canonical modes)
flags.c_cc[VMIN] = 0; // i.e. min 1 char for blocking, 0 chars for non-blocking
flags.c_cc[VTIME] = 0; // block if waiting for char
tcsetattr(fd,TCSANOW,&flags);
FD_ZERO(&stdin_fdset_);
FD_SET(fd, &stdin_fdset_);
maxfd_ = fd + 1;
char time[1024];
bool has_time = false;
int option_char;
bool try_future = false;
while ((option_char = getopt(i_argc,i_argv,"ncahpb:r:s:t:q:T")) != -1){
switch (option_char){
case 'c': ros::shutdown(); break; // This shouldn't happen
case 'n': quiet_ = true; break;
case 'a': at_once_ = true; break;
case 'p': paused_ = true; break;
case 's': advertise_sleep_ = ros::WallDuration(atof(optarg)); break;
case 't': strncpy(time,optarg,sizeof(time)); has_time=true; break;
case 'q': queue_size_ = atoi(optarg); break;
case 'b': bag_time_frequency_ = atoi(optarg); bag_time_ = true; break;
case 'r': time_scale_ = atof(optarg); break;
case 'T': try_future = true; break;
case 'h': print_help(); ros::shutdown(); return;
case '?': print_usage(); ros::shutdown(); return;
}
}
//std::cout << "check options" << std::endl;
float float_time = 0.0f;
if (has_time)
float_time = atof(time);
if (optind == i_argc){
fprintf(stderr, "You must specify at least one bagfile to play from.\n");
print_help(); ros::shutdown(); return;
}
std::vector<std::string> bags;
for (int i = optind; i < i_argc; i++)
bags.push_back(i_argv[i]);
if (bag_time_ && bags.size() > 1){
fprintf(stderr, "You can only play one single bag when using bag time [-b].\n");
print_usage(); ros::shutdown(); return;
}
node_handle = new ros::NodeHandle;
if (bag_time_)
bag_time_publisher_ = new SimpleTimePublisher(bag_time_frequency_, time_scale_);
else
bag_time_publisher_ = new SimpleTimePublisher(-1.0, time_scale_);
start_time_ = ros::WallTime::now();
requested_start_time_ = start_time_;
if (player_.open(bags, ros::Time(start_time_.sec, start_time_.nsec) + ros::Duration().fromSec(-float_time / time_scale_), time_scale_, try_future))
player_.addHandler<AnyMsg>(string("*"), &RosPlay::doPublish, this, NULL, false);
else
{
// Not sure exactly why, but this shutdown is necessary for the
// exception to be received by the main thread without giving a Ctrl-C.
ros::shutdown();
throw std::runtime_error("Failed to open one of the bag files.");
}
bag_time_publisher_->setTime(player_.getFirstTime());
if (!at_once_){
if (paused_){
paused_time_ = ros::WallTime::now();
std::cout << "Hit space to resume, or 's' to step.";
std::cout.flush();
} else {
std::cout << "Hit space to pause.";
std::cout.flush();
}
}
}
RosPlay::~RosPlay()
{
if (node_handle)
{
// This sleep shouldn't be necessary
usleep(1000000);
delete node_handle;
}
if (bag_time_publisher_)
delete bag_time_publisher_;
// Fix terminal settings.
const int fd = fileno(stdin);
tcsetattr(fd,TCSANOW,&orig_flags_);
}
bool RosPlay::spin()
{
if (node_handle && node_handle->ok()){
if(!quiet_)
puts("");
ros::WallTime last_print_time(0.0);
ros::WallDuration max_print_interval(0.1);
while (node_handle->ok()){
if (!player_.nextMsg())
break;
ros::WallTime t = ros::WallTime::now();
if(!quiet_ && ((t - last_print_time) >= max_print_interval))
{
printf("Time: %16.6f Duration: %16.6f\r",
ros::WallTime::now().toSec(), player_.getDuration().toSec());
fflush(stdout);
last_print_time = t;
}
}
std::cout << std::endl << "Done." << std::endl;
// Request a shutdown
ros::shutdown();
}
return true;
}
std::map<std::string, ros::Publisher> g_publishers;
void RosPlay::doPublish(string topic_name, ros::Message* m, ros::Time _play_time, ros::Time record_time, void* n)
{
ros::WallTime play_time(_play_time.sec, _play_time.nsec);
if (play_time < requested_start_time_)
{
bag_time_publisher_->setTime(record_time);
return;
}
// We pull latching and callerid info out of the connection_header if it's available (which it always should be)
bool latching = false;
std::string callerid("");
if (m->__connection_header != NULL)
{
M_string::iterator latch_iter = m->__connection_header->find(std::string("latching"));
if (latch_iter != m->__connection_header->end())
{
if (latch_iter->second != std::string("0"))
{
latching = true;
}
}
M_string::iterator callerid_iter = m->__connection_header->find(std::string("callerid"));
if (callerid_iter != m->__connection_header->end())
{
callerid = callerid_iter->second;
}
}
// We make a unique id composed of the callerid and the topicname allowing us to have separate advertisers
// for separate latching topics.
std::string name = callerid + topic_name;
std::map<std::string, ros::Publisher>::iterator pub_token = g_publishers.find(name);
bag_time_publisher_->setWCHorizon(play_time);
bag_time_publisher_->setHorizon(record_time);
// advertise the topic to publish
if (pub_token == g_publishers.end())
{
AdvertiseOptions opts(topic_name, queue_size_, m->__getMD5Sum(), m->__getDataType(), m->__getMessageDefinition());
// Have to set the latching field explicitly
opts.latch = latching;
ros::Publisher pub = node_handle->advertise(opts);
g_publishers.insert(g_publishers.begin(), std::pair<std::string, ros::Publisher>(name, pub));
pub_token = g_publishers.find(name);
// ROS_INFO("Sleeping %.3f seconds after advertising %s...",
// advertise_sleep_.toSec(), topic_name.c_str());
bag_time_publisher_->runStalledClock(advertise_sleep_);
// ROS_INFO("Done sleeping.\n");
player_.shiftTime(ros::Duration(advertise_sleep_.sec, advertise_sleep_.nsec));
play_time += advertise_sleep_;
bag_time_publisher_->setWCHorizon(play_time);
}
if (!at_once_){
while ( (paused_ || !bag_time_publisher_->horizonReached()) && node_handle->ok()){
bool charsleftorpaused = true;
while (charsleftorpaused && node_handle->ok()){
//Read from stdin:
char c = EOF;
#ifdef __APPLE__
fd_set testfd;
FD_COPY(&stdin_fdset_, &testfd);
#else
fd_set testfd = stdin_fdset_;
#endif
timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 0;
if (select(maxfd_, &testfd, NULL, NULL, &tv) > 0)
c = getc(stdin);
switch (c){
case ' ':
paused_ = !paused_;
if (paused_) {
paused_time_ = ros::WallTime::now();
std::cout << std::endl << "Hit space to resume, or 's' to step.";
std::cout.flush();
} else {
ros::WallDuration shift = ros::WallTime::now() - paused_time_;
paused_time_ = ros::WallTime::now();
player_.shiftTime(ros::Duration(shift.sec, shift.nsec));
play_time += shift;
bag_time_publisher_->setWCHorizon(play_time);
std::cout << std::endl << "Hit space to pause.";
std::cout.flush();
}
break;
case 's':
if (paused_){
bag_time_publisher_->stepClock();
ros::WallDuration shift = ros::WallTime::now() - play_time ;
paused_time_ = ros::WallTime::now();
player_.shiftTime(ros::Duration(shift.sec, shift.nsec));
play_time += shift;
bag_time_publisher_->setWCHorizon(play_time);
(pub_token->second).publish(*m);
return;
}
break;
case EOF:
if (paused_)
bag_time_publisher_->runStalledClock(ros::WallDuration(.01));
else
charsleftorpaused = false;
}
}
bag_time_publisher_->runClock(ros::WallDuration(.01));
}
} else {
bag_time_publisher_->stepClock();
}
(pub_token->second).publish(*m);
}
struct BagContent
{
string datatype;
string md5sum;
string definition;
int count;
BagContent(string d, string m, string def) : datatype(d), md5sum(m), definition(def), count(1) {}
};
map<string, BagContent> g_content;
unsigned long long g_end_time;
void checkFile(string name, ros::Message* m, ros::Time time_play, ros::Time time_recorded, void* n)
{
map<string, BagContent>::iterator i = g_content.find(name);
if (i == g_content.end())
{
g_content.insert(pair<string, BagContent>(name, BagContent(m->__getDataType(), m->__getMD5Sum(), m->__getMessageDefinition())));
} else {
i->second.count++;
}
g_end_time = time_play.toNSec();
}
int main(int argc, char **argv)
{
bool check_bag = false; // We intercept this before starting the node since checkbag doesn't require a node
bool show_defs = false;
for (int i = 0; i < argc; i++)
{
if (!strcmp(argv[i],"-c") || !strcmp(argv[i],"-cd"))
{
check_bag = true;
}
}
if (check_bag)
{
bool try_future = false;
int option_char;
while ((option_char = getopt(argc,argv,"cdahpt:q:T")) != -1)
switch (option_char)
{
case 'c': break;
case 'd': show_defs = true; break;
case 'a': fprintf(stderr, "Option -a is not valid when checking bag\n"); return 1;
case 'p': fprintf(stderr, "Option -p is not valid when checking bag\n"); return 1;
case 't': fprintf(stderr, "Option -t is not valid when checking bag\n"); return 1;
case 'q': fprintf(stderr, "Option -q is not valid when checking bag\n"); return 1;
case 'T': try_future = true; break;
case 'h': print_help(); return 0;
case '?': print_usage(); return 0;
}
if (argc - optind > 1)
{
fprintf(stderr, "Only 1 bag can be checked at a time\n");
return 1;
}
ros::record::Player player;
if (player.open(argv[optind], ros::Time(), try_future))
{
player.addHandler<AnyMsg>(string("*"), &checkFile, NULL, false);
}
while(player.nextMsg())
{
}
printf("bag: %s\n", argv[optind]);
printf("version: %s\n", player.getVersionString().c_str());
printf("start_time: %lld\n", (long long int)player.getFirstDuration().toNSec());
printf("end_time: %lld\n", g_end_time + player.getFirstDuration().toNSec());
printf("length: %lld\n", g_end_time);
printf("topics:\n");
for (map<string, BagContent>::iterator i = g_content.begin();
i != g_content.end();
i++)
{
printf(" - name: %s\n", i->first.c_str());
printf(" count: %d\n", i->second.count);
printf(" datatype: %s\n", i->second.datatype.c_str());
printf(" md5sum: %s\n", i->second.md5sum.c_str());
if (show_defs)
{
string def = i->second.definition.c_str();
if (def.length() > 0)
{
printf(" definition: |\n");
size_t oldind = 0;
size_t ind = def.find_first_of('\n', 0);
while (ind != def.npos)
{
printf(" %s\n", def.substr(oldind, ind - oldind).c_str());
oldind = ind + 1;
ind = def.find_first_of('\n', oldind);
}
ind = def.length();
printf(" %s\n", def.substr(oldind, ind - oldind).c_str());
} else {
printf(" definition: NONE\n");
}
}
}
return 0;
} // if check bag
ros::init(argc, argv, "rosplay", ros::init_options::AnonymousName);
ROS_WARN("Rosplay has been deprecated. Please use 'rosbag play' instead\n");
try
{
RosPlay rp(argc, argv);
rp.spin();
}
catch(std::runtime_error& e)
{
ROS_FATAL("%s", e.what());
return 1;
}
return 0;
}

320
tools/rosrecord/src/rosplay/time_publisher.cpp Normal file
View File

@ -0,0 +1,320 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#include "rosrecord/time_publisher.h"
#include <ros/time.h>
#include "roslib/Clock.h"
SimpleTimePublisher::SimpleTimePublisher(double publish_frequency, double time_scale)
{
if (publish_frequency > 0)
{
do_publish_ = true;
} else {
do_publish_ = false;
}
publish_frequency_ = publish_frequency;
time_scale_ = time_scale;
wall_step_.fromSec(1.0 / publish_frequency);
time_pub_ = node_handle_.advertise<roslib::Clock>("clock",1);
}
void SimpleTimePublisher::setHorizon(const ros::Time& horizon)
{
horizon_ = horizon;
}
void SimpleTimePublisher::setWCHorizon(const ros::WallTime& horizon)
{
wc_horizon_ = horizon;
}
void SimpleTimePublisher::setTime(const ros::Time& time)
{
current_ = time;
}
void SimpleTimePublisher::runClock(const ros::WallDuration& duration)
{
if (do_publish_)
{
roslib::Clock pub_msg;
ros::WallTime t = ros::WallTime::now();
ros::WallTime done = t + duration;
while ( t < done && t < wc_horizon_ )
{
ros::WallDuration leftHorizonWC = wc_horizon_ - t;
ros::Duration d(leftHorizonWC.sec, leftHorizonWC.nsec);
d *= time_scale_;
current_ = horizon_ - d;
if (current_ >= horizon_)
current_ = horizon_;
if (t >= next_pub_)
{
pub_msg.clock = current_;
time_pub_.publish(pub_msg);
next_pub_ = t + wall_step_;
}
ros::WallTime target = done;
if (target > wc_horizon_)
target = wc_horizon_;
if (target > next_pub_)
target = next_pub_;
ros::WallTime::sleepUntil(target);
t = ros::WallTime::now();
}
} else {
ros::WallTime target = ros::WallTime::now() + duration;
if (target > wc_horizon_)
target = wc_horizon_;
ros::WallTime::sleepUntil(target);
}
}
void SimpleTimePublisher::stepClock()
{
if (do_publish_)
{
current_ = horizon_;
roslib::Clock pub_msg;
pub_msg.clock = current_;
time_pub_.publish(pub_msg);
ros::WallTime t = ros::WallTime::now();
next_pub_ = t + wall_step_;
} else {
current_ = horizon_;
}
}
void SimpleTimePublisher::runStalledClock(const ros::WallDuration& duration)
{
if (do_publish_)
{
roslib::Clock pub_msg;
ros::WallTime t = ros::WallTime::now();
ros::WallTime done = t + duration;
while ( t < done )
{
if (t > next_pub_)
{
pub_msg.clock = current_;
time_pub_.publish(pub_msg);
next_pub_ = t + wall_step_;
}
ros::WallTime target = done;
if (target > next_pub_)
target = next_pub_;
ros::WallTime::sleepUntil(target);
t = ros::WallTime::now();
}
} else {
duration.sleep();
}
}
bool SimpleTimePublisher::horizonReached()
{
return ros::WallTime::now() > wc_horizon_;
}
TimePublisher::TimePublisher():
freeze_time_(true),
is_started_(false),
publish_thread_(NULL),
time_scale_factor_(1.0),
continue_(true)
{}
TimePublisher::~TimePublisher()
{
if (publish_thread_){
continue_ = false;
publish_thread_->join(); // Wait for the thread to die before I kill the TimePublisher.
delete publish_thread_;
}
}
void TimePublisher::initialize(double publish_frequency, double time_scale_factor)
{
// set frequency to publish
publish_freq_ = publish_frequency;
// We can't publish any time yet
horizon_.fromSec(0.0);
// advertise time topic
time_pub = node_handle.advertise<roslib::Clock>("clock",1);
publish_thread_ = new boost::thread(boost::bind(&TimePublisher::publishTime, this));
time_scale_factor_ = time_scale_factor;
}
// Freeze time by repeatedly publishing the same time.
void TimePublisher::freezeTime()
{
// lock
boost::mutex::scoped_lock offset_lock(offset_mutex_);
freeze_time_ = true;
horizon_.fromSec(0.0);
}
// start time at bag timepoint bag_time
void TimePublisher::startTime(ros::Time bag_time)
{
// lock
boost::mutex::scoped_lock offset_lock(offset_mutex_);
// time is not frozen any more
freeze_time_ = false;
// get time we should publish
last_pub_time_ = bag_time;
// get system time when started
last_sys_time_ = getSysTime();
is_started_ = true;
}
void TimePublisher::stepTime(ros::Time bag_time)
{
assert(freeze_time_);
// lock
boost::mutex::scoped_lock offset_lock(offset_mutex_);
// get time we should publish;
last_pub_time_ = bag_time;
// get system time when started
last_sys_time_ = getSysTime();
// Reset horizon, to allow for publication of a time message while
// stepping.
horizon_ = last_sys_time_;
is_started_ = true;
}
// Update the time up to which the publisher is allowed to run
void TimePublisher::setHorizon(ros::Time& horizon)
{
boost::mutex::scoped_lock offset_lock(offset_mutex_);
horizon_ = horizon;
}
// Accessor, with lock
const ros::Time& TimePublisher::getHorizon()
{
boost::mutex::scoped_lock offset_lock(offset_mutex_);
return horizon_;
}
// get the system time
ros::Time TimePublisher::getSysTime()
{
struct timeval timeofday;
gettimeofday(&timeofday,NULL);
return ros::Time().fromNSec(1e9*timeofday.tv_sec + 1e3*timeofday.tv_usec);
}
// publish time
void TimePublisher::publishTime()
{
roslib::Clock pub_msg;
while(node_handle.ok() && continue_) {
if (is_started_){
ros::Time now = getSysTime();
// Are we allowed to publish this time yet?
while(((now - getHorizon()) > ros::Duration(0,100000)) && node_handle.ok() && continue_)
{
usleep(1000);
now = getSysTime();
}
{
boost::mutex::scoped_lock offset_lock(offset_mutex_);
pub_msg.clock = last_pub_time_ +
((now - last_sys_time_) * time_scale_factor_);
last_sys_time_ = now;
last_pub_time_ = pub_msg.clock;
}
time_pub.publish(pub_msg);
}
usleep(1e6 / (publish_freq_ * time_scale_factor_));
}
}

322
tools/rosrecord/src/rosrecord.py Normal file
View File

@ -0,0 +1,322 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
PKG = 'rosrecord'
import roslib; roslib.load_manifest(PKG)
import optparse
import os
import sys
import rospy
import rosbag
from bisect import bisect
class ROSRecordException(Exception):
"""
Base exception type for rosrecord-related errors.
"""
def __init__(self, value):
self.value = value
def __str__(self):
return repr(self.value)
class BagReader(object):
"""
Bag file reader
"""
def __init__(self, f):
"""
Open a bag file for reading
@param f: either a path to bag file to open for reading, or a stream to read from
@type f: str or file
@raise ROSRecordException: if version not supported
"""
try:
self._bag = rosbag.Bag(f, 'r')
except Exception, e:
raise ROSRecordException(str(e))
def close(self):
"""
Close the bag file
"""
try:
if self._bag:
self._bag.close()
except Exception, e:
raise ROSRecordException(str(e))
@property
def datatypes(self): return dict([(info.topic, info.datatype) for info in self._bag._connections.values()])
@property
def filename(self): return self._bag.filename
def read_index(self):
"""
Read the index from the file (if it exists)
@return dict(topic, [(stamp, pos)...]): bag file index
"""
try:
index = {}
connection_index = self._bag.get_index()
for (connection_id, connection_topic, connection_datatype), entries in connection_index.items():
if connection_topic not in index:
index[connection_topic] = []
topic_index = index[connection_topic]
for entry in entries:
topic_index.insert(bisect(topic_index, entry), entry)
self.index = self._bag.get_index()
except rosbag.ROSBagException:
self.index = None
return self.index
def logplayer(self, raw=False, seek=None):
if not self._bag:
return
if seek is not None:
raise ROSRecordException('seek not supported')
try:
try:
for topic, msg, t in self._bag.read_messages(raw=raw):
if msg is None or rospy.is_shutdown():
break
yield topic, msg, t
except KeyboardInterrupt:
pass # break iterator
finally:
self._bag.close()
def raw_messages(self):
"""
Generates raw messages
@return (pos, topic, msg, t)...: tuple of position, topic, message data and time stamp
"""
try:
for topic, msg, t in self._bag.read_messages(raw=True):
if msg is None or rospy.is_shutdown():
break
(datatype, data, md5sum, position, pytype) = msg
yield position, topic, msg, t
except rosbag.ROSBagException, e:
rospy.logerr('ROSBagException: couldn\'t read message - %s' % str(e))
except IOError:
rospy.logerr('IOError: couldn\'t read message')
except KeyError:
rospy.logerr('KeyError: couldn\'t read message')
def load_message(self, pos, index=None):
"""
Load message from the bag file at a given position
@param pos: byte offset in file of start of message record
@type pos: int
@return (datatype, msg, t): tuple of datatype, deserialized message, and timestamp
"""
msgs = list(self.load_messages([pos], index))
if len(msgs) == 0:
return None, None, None
return msgs[0]
## Returns iterator of (datatype, message, timestamp)
def load_messages(self, positions, index=None):
try:
for pos in positions:
_, raw_msg, t = self._bag._read_message(pos, True)
if raw_msg is not None:
(datatype, message_data, md5, bag_pos, pytype) = raw_msg
msg = pytype()
msg.deserialize(message_data)
yield (datatype, msg, t)
except rosbag.ROSBagException, e:
rospy.logerr('ROSRecordException: couldn\'t read %s - %s' % (str(pos), str(e)))
except IOError:
rospy.logerr('IOError: couldn\'t read %d' % pos)
except KeyError:
rospy.logerr('KeyError: couldn\'t read %d' % pos)
def logplayer(f, raw=False, seek=None):
"""
Iterator for messages in a bag file. Return type of iterator depends on value of 'raw' parameter, which
is provided for improved iteration performance.
If raw is False:
- topic: topic name (str)
- msg: Message instance (Message)
- t: time (rospy.Time)
If raw is True:
- topic: topic name (str)
- msg: (datatype, data, md5sum, bag_position).
- datatype: msg type name
- data: serialized message bytes.
- bag_position: offset into the file, which can be used for seeking.
- md5sum: md5sum of msg type for identify msg version
- NOTE: More elements may be added to this in the future, so this should
be unpacked assuming non-fixed length.
- t: time (rospy.Time)
@param filename: name of file to playback from
@type filename: str
@param raw: see description for raw behavior
@type raw: bool
@param seek: initial position to seek to (default None)
@type seek: int
@raise ROSRecordException: if file format is unrecognized
@return: iterator for messages. See description for iterator behavior.
@rtype: iterator
"""
r = BagReader(f)
return r.logplayer(raw, seek)
class Rebagger(object):
"""
Utility class for writing Message instances to a ROS .bag file
"""
def __init__(self, filename):
self._bag = rosbag.Bag(filename, 'w')
def add(self, topic, msg, t=None, raw=False):
"""
Add a message to the bag
@param msg: message to add to bag
@type msg: Message
@param raw: if True, msg is in raw format (msg_type, serialized_bytes, md5sum, pytype)
@param raw: bool
@param t: ROS time of message publication
@type t: U{roslib.message.Time}
"""
try:
self._bag.write(topic, msg, t, raw)
except:
raise ROSRecordException('error adding message to bag')
def close(self):
try:
if self._bag:
self._bag.close()
except:
raise ROSRecordException('error closing Rebagger')
def rebag(inbag, outbag, filter_fn, verbose_pattern=None, raw=False):
"""
Filter the contents of inbag to outbag using filter_fn
@param inbag: filename of input bag file.
@type inbag: str
@param outbag: filename of output bag file. Existing bag file will be overwritten
@type outbag: str
@param filter_fn: Python function that returns True if msg is to be kept
@type filter_fn: fn(topic, msg, time)
@param raw: if True, msg will be kept deserialized. This is useful
if you are removing messages that no longer exist.
@type raw: bool
@param verbose_pattern: Python function that returns string to
print for verbose debugging
@type verbose_pattern: fn(topic, msg, time)
"""
rebag = Rebagger(outbag)
if verbose_pattern:
for topic, msg, t in logplayer(inbag, raw=raw):
if filter_fn(topic, msg, t):
print "MATCH", verbose_pattern(topic, msg, t)
rebag.add(topic, msg, t, raw=raw)
if rospy.is_shutdown():
break
else:
print "NO MATCH", verbose_pattern(topic, msg, t)
else: #streamlined
for topic, msg, t in logplayer(inbag, raw=raw):
if filter_fn(topic, msg, t):
rebag.add(topic, msg, t, raw=raw)
if rospy.is_shutdown():
break
rebag.close()
def _rebag_main():
"""
main routine for rosrebag command-line tool
"""
rospy.logwarn("Rosrebag has been deprecated. Please use 'rosbag filter' instead\n")
## filter function that uses command line expression to test topic/message/time
def expr_eval(expr):
def eval_fn(topic, m, t):
return eval(expr)
return eval_fn
parser = optparse.OptionParser(usage="""Usage: %prog in.bag out.bag filter-expression
filter-expression can be any Python-legal expression.
The following variables are available:
* topic: name of topic
* m: message
* t: time of message (t.secs, t.nsecs)
""", prog='rosrebag')
parser.add_option('--print', dest="verbose_pattern", default=None,
metavar="PRINT-EXPRESSION", help="Python expression to print for verbose debugging. Uses same variables as filter-expression.")
options, args = parser.parse_args()
if len(args) == 0:
parser.print_usage()
sys.exit(0)
elif len(args) != 3:
parser.error("invalid arguments")
inbag, outbag, expr = args
if options.verbose_pattern:
verbose_pattern = expr_eval(options.verbose_pattern)
else:
verbose_pattern = None
if not os.path.isfile(inbag):
print >> sys.stderr, "cannot locate input bag file [%s]" % inbag
sys.exit(1)
rebag(inbag, outbag, expr_eval(expr), verbose_pattern=verbose_pattern)

182
tools/rosrecord/src/rosrecord/recorder.cpp Normal file
View File

@ -0,0 +1,182 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "rosrecord/constants.h"
#include <iomanip>
#include <signal.h>
#include <sys/statvfs.h>
#include <boost/filesystem.hpp>
#include <boost/iostreams/filter/gzip.hpp>
#include <boost/iostreams/filter/bzip2.hpp>
#include "ros/message.h"
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
#define IGNORE_ROSRECORD_DEPRECATED
#include "rosrecord/Recorder.h"
using std::string;
namespace ros {
namespace record {
Recorder::Recorder() :
logging_enabled_(true)
{
}
Recorder::~Recorder()
{
close();
}
bool Recorder::open(string const& file_name, bool random_access)
{
try
{
bag_.open(file_name, rosbag::bagmode::Write);
check_disk_next_ = ros::WallTime::now() + ros::WallDuration().fromSec(20.0);
warn_next_ = ros::WallTime();
checkDisk();
return true;
}
catch (rosbag::BagException ex)
{
ROS_FATAL("rosrecord::Record: Failed to open file: %s", file_name.c_str());
return false;
}
}
pos_t Recorder::getOffset() { return bag_.getSize(); }
void Recorder::close()
{
bag_.close();
}
bool Recorder::checkDisk()
{
struct statvfs fiData;
string filename = bag_.getFileName();
if ((statvfs(filename.c_str(), &fiData)) < 0)
{
ROS_WARN("rosrecord::Record: Failed to check filesystem stats.");
}
else
{
unsigned long long free_space = 0;
free_space = (unsigned long long)(fiData.f_bsize) * (unsigned long long)(fiData.f_bavail);
if (free_space < 1073741824ull)
{
ROS_ERROR("rosrecord::Record: Less than 1GB of space free on disk with %s. Disabling logging.", filename.c_str());
logging_enabled_ = false;
return false;
}
else if (free_space < 5368709120ull)
{
ROS_WARN("rosrecord::Record: Less than 5GB of space free on disk with %s.", filename.c_str());
}
else
{
logging_enabled_ = true;
}
}
return true;
}
bool Recorder::record(string topic_name, ros::Message::ConstPtr msg, ros::Time time)
{
return record(topic_name, *msg, time);
}
bool Recorder::record(string topic_name, const ros::Message& msg, ros::Time time)
{
if (!logging_enabled_)
{
ros::WallTime nowtime = ros::WallTime::now();
if (nowtime > warn_next_)
{
warn_next_ = nowtime + ros::WallDuration().fromSec(5.0);
ROS_WARN("Not logging message because logging disabled. Most likely cause is a full disk.");
}
return false;
}
{
boost::mutex::scoped_lock lock(check_disk_mutex_);
if (ros::WallTime::now() > check_disk_next_)
{
check_disk_next_ = check_disk_next_ + ros::WallDuration().fromSec(20.0);
if (!checkDisk())
return false;
}
}
boost::shared_ptr<ros::M_string> hdr = msg.__connection_header;
if (hdr == NULL)
hdr = boost::shared_ptr<ros::M_string>(new ros::M_string);
(*hdr)["type"] = std::string(ros::message_traits::datatype(msg));
(*hdr)["md5sum"] = std::string(ros::message_traits::md5sum(msg));
(*hdr)["message_definition"] = std::string(ros::message_traits::definition(msg));
if (hdr->find("callerid") == hdr->end())
(*hdr)["callerid"] = string("");
if (hdr->find("latching") == hdr->end())
(*hdr)["latching"] = string("0");
try
{
bag_.write(topic_name, time, msg, hdr);
}
catch (rosbag::BagException ex)
{
ROS_FATAL("rosrecord::Record: could not write to file. Check permissions and diskspace\n");
return false;
}
return true;
}
} // namespace record
} // namespace ros

565
tools/rosrecord/src/rosrecord/rosrecord.cpp Normal file
View File

@ -0,0 +1,565 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include <time.h>
#include <sys/stat.h>
#include "ros/ros.h"
#include "topic_tools/shape_shifter.h"
#include <boost/thread.hpp>
#include <boost/lexical_cast.hpp>
#include <sstream>
#include <string>
#include <set>
#include <queue>
#include "std_msgs/Empty.h"
// We know this is deprecated..
#undef ROSCPP_DEPRECATED
#define ROSCPP_DEPRECATED
#define IGNORE_ROSRECORD_DEPRECATED
#include "rosrecord/Recorder.h"
class OutgoingMessage
{
public:
OutgoingMessage(std::string _topic_name, topic_tools::ShapeShifter::ConstPtr _msg, ros::Time _time) :
topic_name(_topic_name), msg(_msg), time(_time) {}
std::string topic_name;
topic_tools::ShapeShifter::ConstPtr msg;
ros::Time time;
};
class OutgoingQueue
{
public:
OutgoingQueue(std::string _fname, std::queue<OutgoingMessage>* _queue, ros::Time _time) :
fname(_fname), queue(_queue), time(_time) {}
std::string fname;
std::queue<OutgoingMessage>* queue;
ros::Time time;
};
//! Global verbose flag so we can easily use from callback
bool g_verbose = false;
//! Global snapshot flag so we can easily use from callback
bool g_snapshot = false;
// Global Eventual exit code
int g_exit_code = 0;
//! Global variable including the set of currenly recording topics
std::set<std::string> g_currently_recording;
//! Global variable used for initialization of counting messages
int g_count = -1;
//! Global variable used for book-keeping of our number of subscribers
int g_num_subscribers = 0;
//! Global queue for storing
std::queue<OutgoingMessage>* g_queue;
//! Global queue size
uint64_t g_queue_size=0;
//! Global max queue size
uint64_t g_max_queue_size=1048576*256;
//! Global split size
uint64_t g_split_size=0;
//! Global split count
uint64_t g_split_count=0;
//! Queue of queues to be used by the snapshot recorders
std::queue<OutgoingQueue> g_queue_queue;
//! Mutex for global queue
boost::mutex g_queue_mutex;
//! Conditional variable for global queue
boost::condition_variable_any g_queue_condition;
//! Global flag to add date
bool g_add_date = true;;
//! Global prefix
std::string g_prefix;
//! Compression mode
static enum {
COMPRESSION_NONE,
COMPRESSION_GZIP,
COMPRESSION_BZIP2
} g_compression = COMPRESSION_NONE;
template <class T>
std::string time_to_str(T ros_t)
{
char buf[1024] = "";
time_t t = ros_t.sec;
struct tm *tms = localtime(&t);
strftime(buf, 1024, "%Y-%m-%d-%H-%M-%S", tms);
return std::string(buf);
}
//! Helper function to print executable usage
void print_usage() {
fprintf (stderr, "Usage: rosrecord [options] TOPIC1 [TOPIC2 TOPIC3...]\n"
" rosrecord logs ROS message data to a file.\n"
);
}
//! Helper function to print executable options
void print_help() {
print_usage();
fprintf(stderr, "Options:\n");
fprintf(stderr, " -c <num> : Only receive <num> messages on each topic\n");
fprintf(stderr, " -f <prefix> : Prepend file prefix to beginning of bag name (name will always end with date stamp)\n");
fprintf(stderr, " -F <fname> : Record to a file named exactly <fname>.bag\n");
fprintf(stderr, " -a : Record all published messages.\n");
fprintf(stderr, " -v : Display a message every time a message is received on a topic\n");
fprintf(stderr, " -m : Maximize internal buffer size in MB (Default: 256MB) 0 = infinite.\n");
// Removing compression until we work out how to play nicely with index: JML
// fprintf(stderr, " -z : Compress the messages with gzip\n");
// fprintf(stderr, " -j : Compress the messages with bzip2\n");
fprintf(stderr, " -s : (EXPERIMENTAL) Enable snapshot recording (don't write to file unless triggered)\n");
fprintf(stderr, " -t : (EXPERIMENTAL) Trigger snapshot recording\n");
fprintf(stderr, " -h : Display this help message\n");
}
//! Callback to be invoked to save messages into a queue
void do_queue(topic_tools::ShapeShifter::ConstPtr msg,
std::string topic_name,
boost::shared_ptr<ros::Subscriber> subscriber,
boost::shared_ptr<int> count)
{
ros::Time rectime = ros::Time::now();
if (g_verbose)
std::cout << "Received message on topic " << subscriber->getTopic() << std::endl;
OutgoingMessage out(topic_name, msg, rectime);
{
boost::mutex::scoped_lock lock(g_queue_mutex);
g_queue->push(out);
g_queue_size += out.msg->size();
while (g_max_queue_size > 0 && g_queue_size > g_max_queue_size)
{
OutgoingMessage drop = g_queue->front();
g_queue->pop();
g_queue_size -= drop.msg->size();
if (!g_snapshot)
{
static ros::Time last = ros::Time();
ros::Time now = ros::Time::now();
if (now > last + ros::Duration(5.0))
{
ROS_WARN("Rosrecord buffer exceeded. Dropping oldest queued message.");
last = now;
}
}
}
}
if (!g_snapshot)
g_queue_condition.notify_all();
// If we are book-keeping count, decrement and possibly shutdown
if ((*count) > 0)
{
(*count)--;
if ((*count) == 0)
{
subscriber->shutdown();
if (--g_num_subscribers == 0)
ros::shutdown();
}
}
}
//! Callback to be invoked to actually do the recording
void snapshot_trigger(std_msgs::Empty::ConstPtr trigger)
{
ros::WallTime rectime = ros::WallTime::now();
std::vector<std::string> join;
if (g_prefix.length() > 0)
join.push_back(g_prefix);
if (g_add_date)
join.push_back(time_to_str(rectime));
std::string tgt_fname = join[0];
for (size_t i = 1; i < join.size(); i++)
tgt_fname = tgt_fname + "_" + join[i];
tgt_fname = tgt_fname + std::string(".bag");
ROS_INFO("Triggered snapshot recording with name %s.", tgt_fname.c_str());
{
boost::mutex::scoped_lock lock(g_queue_mutex);
OutgoingQueue out(tgt_fname, g_queue, ros::Time::now());
g_queue = new std::queue<OutgoingMessage>;
g_queue_size=0;
g_queue_queue.push(out);
}
g_queue_condition.notify_all();
}
//! Thread that actually does writing to file.
void do_record()
{
ros::WallTime rectime = ros::WallTime::now();
std::vector<std::string> join;
if (g_prefix.length() > 0)
join.push_back(g_prefix);
if (g_add_date)
join.push_back(time_to_str(rectime));
std::string base_name = join[0];
for (size_t i = 1; i < join.size(); i++)
base_name = base_name + "_" + join[i];
std::string split_name("");
if (g_split_size > 0)
split_name = std::string("_") + boost::lexical_cast<std::string>(g_split_count++);
std::string suffix;
// Removing compression until we work out how to play nicely with index: JML
/*
switch (g_compression) {
case COMPRESSION_GZIP:
suffix = ".gz";
break;
case COMPRESSION_BZIP2:
suffix = ".bz2";
break;
case COMPRESSION_NONE:
break;
default:
ROS_FATAL("Unknown compression method requested: %d", g_compression);
g_exit_code = 1;
ros::shutdown();
break;
}
*/
std::string tgt_fname = base_name + split_name + std::string(".bag") + suffix;
std::string fname = base_name + split_name + std::string(".bag.active") + suffix;
ROS_INFO("Recording to %s.", tgt_fname.c_str());
ros::NodeHandle nh;
ros::record::Recorder recorder;
// Open our recorder and add available topics
if (!recorder.open(std::string(fname)))
{
ROS_FATAL("Could not open output file: %s", fname.c_str());
g_exit_code = 1;
ros::shutdown();
}
// Technically the g_queue_mutex should be locked while checking empty
// Except it should only get checked if the node is not ok, and thus
// it shouldn't be in contention.
while (nh.ok() || !g_queue->empty())
{
boost::unique_lock<boost::mutex> lock(g_queue_mutex);
bool finished = false;
while (g_queue->empty())
{
if (!nh.ok())
{
lock.release()->unlock();
finished = true;
break;
}
g_queue_condition.wait(lock);
}
if (finished)
break;
OutgoingMessage out = g_queue->front();
g_queue->pop();
g_queue_size -= out.msg->size();
lock.release()->unlock();
if (g_split_size > 0 && recorder.getOffset() > g_split_size)
{
recorder.close();
rename(fname.c_str(),tgt_fname.c_str());
split_name = std::string("_") + boost::lexical_cast<std::string>(g_split_count++);
tgt_fname = base_name + split_name + std::string(".bag") + suffix;
fname = base_name + split_name + std::string(".bag.active") + suffix;
if (!recorder.open(std::string(fname)))
{
ROS_FATAL("Could not open output file: %s", fname.c_str());
g_exit_code = 1;
ros::shutdown();
}
ROS_INFO("Recording to %s.", tgt_fname.c_str());
}
recorder.record(out.topic_name, out.msg, out.time);
}
// Close the file nicely
ROS_INFO("Closing %s.", tgt_fname.c_str());
recorder.close();
rename(fname.c_str(), tgt_fname.c_str());
}
void do_record_bb()
{
ros::NodeHandle nh;
while (nh.ok() || !g_queue_queue.empty())
{
boost::unique_lock<boost::mutex> lock(g_queue_mutex);
while (g_queue_queue.empty())
{
if (!nh.ok())
return;
g_queue_condition.wait(lock);
}
OutgoingQueue out_queue = g_queue_queue.front();
g_queue_queue.pop();
lock.release()->unlock();
std::string tgt_fname = out_queue.fname;
std::string fname = tgt_fname + std::string(".active");
ros::record::Recorder recorder;
if (recorder.open(fname))
{
while (!out_queue.queue->empty())
{
OutgoingMessage out = out_queue.queue->front();
out_queue.queue->pop();
recorder.record(out.topic_name, out.msg, out.time);
}
// Close the file nicely
recorder.close();
// Rename the file to the actual target name
rename(fname.c_str(),tgt_fname.c_str());
}
else
{
ROS_ERROR("Could not open file: %s", out_queue.fname.c_str());
}
}
}
void do_check_master(const ros::TimerEvent& e, ros::NodeHandle& node_handle)
{
ros::master::V_TopicInfo all_topics;
if (ros::master::getTopics(all_topics))
{
// For each topic
for (ros::master::V_TopicInfo::iterator topic_iter = all_topics.begin();
topic_iter != all_topics.end();
topic_iter++)
{
// If we're not currently recording it, do so
if (g_currently_recording.find(topic_iter->name) == g_currently_recording.end())
{
boost::shared_ptr<int> count(new int(g_count));
boost::shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
*sub = node_handle.subscribe<topic_tools::ShapeShifter>(topic_iter->name, 100, boost::bind(&do_queue, _1, topic_iter->name, sub, count));
g_currently_recording.insert(topic_iter->name);
}
}
}
}
void do_trigger()
{
// Get a node_handle
ros::NodeHandle node_handle;
ros::Publisher pub = node_handle.advertise<std_msgs::Empty>("snapshot_trigger", 1, true);
pub.publish(std_msgs::Empty());
ros::Timer terminate_timer = node_handle.createTimer(ros::Duration(1.0), boost::bind(&ros::shutdown));
ros::spin();
}
//! Main function
int main(int argc, char **argv)
{
ros::init(argc, argv, "rosrecord", ros::init_options::AnonymousName);
ROS_WARN("Rosrecord has been deprecated. Please use 'rosbag record' instead\n");
// Variables
bool check_master = false; // Whether master should be checked periodically
// Parse options
int option_char;
while ((option_char = getopt(argc,argv,"f:F:c:m:S:asthv")) != -1)
{
switch (option_char)
{
case 'f': g_prefix = std::string(optarg); break;
case 'F': g_prefix = std::string(optarg); g_add_date = false; break;
case 'c': g_count = atoi(optarg); break;
case 'a': check_master = true; break;
case 's': g_snapshot = true; break;
case 't': do_trigger(); return 0; break;
case 'v': g_verbose = true; break;
// Removing compression until we work out how to play nicely with index: JML
// case 'z': g_compression = COMPRESSION_GZIP; break;
// case 'j': g_compression = COMPRESSION_BZIP2; break;
case 'm':
{
int m=0;
m=atoi(optarg);
if (m < 0)
{
fprintf(stderr, "Buffer size must be 0 or positive.\n");
return 1;
}
g_max_queue_size = 1048576*m;
}
break;
case 'S':
{
int S=0;
S=atoi(optarg);
if (S < 0)
{
fprintf(stderr, "Splitting size must be 0 or positive.\n");
return 1;
}
g_split_size = 1048576*S;
}
break;
case 'h': print_help(); return 1;
case '?': print_usage(); return 1;
}
}
if (g_snapshot)
ROS_WARN("Using snapshot mode in rosrecord is experimental and usage syntax is subject to change");
// Logic to make sure count is not specified with automatic topic
// subscription (implied by no listed topics)
if ((argc - optind) < 1)
{
if (g_count > 0)
{
fprintf(stderr, "Specifing a count is not valid with automatic topic subscription.\n");
return 1;
}
if (!check_master)
{
ROS_WARN("Running rosrecord with no arguments has been deprecated. Please use 'rosrecord -a' instead\n");
check_master = true;
}
}
g_queue = new std::queue<OutgoingMessage>;
// Get a node_handle
ros::NodeHandle node_handle;
// Only set up recording if we actually got a useful nodehandle
if (node_handle.ok())
{
boost::thread record_thread;
// Spin up a thread for actually writing to file
if (!g_snapshot)
record_thread = boost::thread(boost::bind(&do_record));
else
record_thread = boost::thread(boost::bind(&do_record_bb));
ros::Subscriber trigger = node_handle.subscribe<std_msgs::Empty>("snapshot_trigger", 100, boost::bind(&snapshot_trigger, _1));
// Every non-processed argument is assumed to be a topic
for (int i = optind; i < argc; i++)
{
boost::shared_ptr<int> count(new int(g_count));
boost::shared_ptr<ros::Subscriber> sub(new ros::Subscriber);
g_num_subscribers++;
*sub = node_handle.subscribe<topic_tools::ShapeShifter>(argv[i], 100, boost::bind(&do_queue, _1, argv[i], sub, count));
}
ros::Timer check_master_timer;
if (check_master)
check_master_timer = node_handle.createTimer(ros::Duration(1.0), boost::bind(&do_check_master, _1, boost::ref(node_handle)));
ros::MultiThreadedSpinner s(10);
ros::spin(s);
g_queue_condition.notify_all();
record_thread.join();
}
delete g_queue;
return g_exit_code;
}

75
tools/rosrecord/test/test_rosrecord_offline.py Executable file
View File

@ -0,0 +1,75 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: test_rostopic_command_line_offline.py 5710 2009-08-20 03:11:04Z sfkwc $
from __future__ import with_statement
NAME = 'test_rosrecord_offline'
import roslib; roslib.load_manifest('rosrecord')
import os
import sys
import unittest
import cStringIO
import time
import rostest
from subprocess import Popen, PIPE, check_call, call
class TestRosrecordOffline(unittest.TestCase):
def setUp(self):
pass
## test that the rosmsg command works
def test_rosrecord_help(self):
self.do_test_help('rosrecord');
def test_rosplay_help(self):
self.do_test_help('rosplay');
def test_rosrecord_pkg_help(self):
self.do_test_help('rosrun rosrecord rosrecord');
def test_rosplay_pkg_help(self):
self.do_test_help('rosrun rosrecord rosplay');
def do_test_help(self, cmd):
rosrecord = Popen(cmd.split() + ['-h'], stdout=PIPE, stderr=PIPE)
output = rosrecord.communicate()
self.assert_('Usage:' in output[0] or 'Usage:' in output[1])
if __name__ == '__main__':
rostest.unitrun('rosrecord', NAME, TestRosrecordOffline, sys.argv, coverage_packages=[])