ml/ar_basalt
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

v01

Browse Source
This commit is contained in:
Ivan
2022-04-05 11:42:28 +03:00
commit 6dc0eb0fcf
5565 changed files with 1200500 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

213
thirdparty/ros/ros_comm/tools/rosbag/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,213 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rosbag
^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.12.14 (2018-08-23)
--------------------
* add TransportHint options --tcpnodelay and --udp (`#1295 <https://github.com/ros/ros_comm/issues/1295>`_)
* fix check for header first in rosbag play for rate control topic (`#1352 <https://github.com/ros/ros_comm/issues/1352>`_)
1.12.13 (2018-02-21)
--------------------
* return an error status on error in rosbag (`#1257 <https://github.com/ros/ros_comm/issues/1257>`_)
* fix warn of --max-splits without --split (`#1237 <https://github.com/ros/ros_comm/issues/1237>`_)
1.12.12 (2017-11-16)
--------------------
1.12.11 (2017-11-07)
--------------------
1.12.10 (2017-11-06)
--------------------
1.12.9 (2017-11-06)
-------------------
1.12.8 (2017-11-06)
-------------------
* fix Python 3 compatibility (`#1150 <https://github.com/ros/ros_comm/issues/1150>`_)
* fix handling connections without indices (`#1109 <https://github.com/ros/ros_comm/pull/1109>`_)
* improve message of check command (`#1067 <https://github.com/ros/ros_comm/pull/1067>`_)
* fix BZip2 inclusion (`#1016 <https://github.com/ros/ros_comm/pull/1016>`_)
1.12.7 (2017-02-17)
-------------------
* throw exception instead of accessing invalid memory (`#971 <https://github.com/ros/ros_comm/pull/971>`_)
* move headers to include/xmlrpcpp (`#962 <https://github.com/ros/ros_comm/issues/962>`_)
* added option wait-for-subscriber to rosbag play (`#959 <https://github.com/ros/ros_comm/issues/959>`_)
* terminate underlying rosbag play, record on SIGTERM (`#951 <https://github.com/ros/ros_comm/issues/951>`_)
* add pause service for rosbag player (`#949 <https://github.com/ros/ros_comm/issues/949>`_)
* add rate-control-topic and rate-control-max-delay. (`#947 <https://github.com/ros/ros_comm/issues/947>`_)
1.12.6 (2016-10-26)
-------------------
* fix BagMigrationException in migrate_raw (`#917 <https://github.com/ros/ros_comm/issues/917>`_)
1.12.5 (2016-09-30)
-------------------
1.12.4 (2016-09-19)
-------------------
1.12.3 (2016-09-17)
-------------------
* set default values for min_space and min_space_str (`#883 <https://github.com/ros/ros_comm/issues/883>`_)
* record a maximum number of splits and then begin deleting old files (`#866 <https://github.com/ros/ros_comm/issues/866>`_)
* allow 64-bit sizes to be passed to robag max_size (`#865 <https://github.com/ros/ros_comm/issues/865>`_)
* update rosbag filter progress meter to use raw uncompressed input size (`#857 <https://github.com/ros/ros_comm/issues/857>`_)
1.12.2 (2016-06-03)
-------------------
1.12.1 (2016-04-18)
-------------------
* promote the result of read_messages to a namedtuple (`#777 <https://github.com/ros/ros_comm/pull/777>`_)
* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
1.12.0 (2016-03-18)
-------------------
* add missing parameter to AdvertiseOptions::createAdvertiseOptions (`#733 <https://github.com/ros/ros_comm/issues/733>`_)
1.11.18 (2016-03-17)
--------------------
1.11.17 (2016-03-11)
--------------------
* use boost::make_shared instead of new for constructing boost::shared_ptr (`#740 <https://github.com/ros/ros_comm/issues/740>`_)
1.11.16 (2015-11-09)
--------------------
* show size unit for --size of rosbag record in help string (`#697 <https://github.com/ros/ros_comm/pull/697>`_)
1.11.15 (2015-10-13)
--------------------
* add option --prefix for prefixing output topics (`#626 <https://github.com/ros/ros_comm/pull/626>`_)
1.11.14 (2015-09-19)
--------------------
* reduce memory usage by using slots for IndexEntry types (`#613 <https://github.com/ros/ros_comm/pull/613>`_)
* remove duplicate topics (`#647 <https://github.com/ros/ros_comm/issues/647>`_)
* better exception when calling get_start_time / get_end_time on empty bags (`#657 <https://github.com/ros/ros_comm/pull/657>`_)
* make support for lz4 in rosbag optional (`#642 <https://github.com/ros/ros_comm/pull/642>`_)
* fix handling of "play --topics" (`#620 <https://github.com/ros/ros_comm/issues/620>`_)
1.11.13 (2015-04-28)
--------------------
1.11.12 (2015-04-27)
--------------------
1.11.11 (2015-04-16)
--------------------
* add support for pausing when specified topics are about to be published (`#569 <https://github.com/ros/ros_comm/pull/569>`_)
1.11.10 (2014-12-22)
--------------------
* add option to specify the minimum disk space at which recording is stopped (`#500 <https://github.com/ros/ros_comm/pull/500>`_)
* add convenience API to Python rosbag (`#508 <https://github.com/ros/ros_comm/issues/508>`_)
* fix delay on detecting a running rosmaster with use_sim_time set (`#532 <https://github.com/ros/ros_comm/pull/532>`_)
1.11.9 (2014-08-18)
-------------------
1.11.8 (2014-08-04)
-------------------
1.11.7 (2014-07-18)
-------------------
1.11.6 (2014-07-10)
-------------------
* fix rosbag record prefix (`#449 <https://github.com/ros/ros_comm/issues/449>`_)
1.11.5 (2014-06-24)
-------------------
* Fix typo in rosbag usage
1.11.4 (2014-06-16)
-------------------
* Python 3 compatibility (`#426 <https://github.com/ros/ros_comm/issues/426>`_, `#430 <https://github.com/ros/ros_comm/issues/430>`_)
1.11.3 (2014-05-21)
-------------------
1.11.2 (2014-05-08)
-------------------
1.11.1 (2014-05-07)
-------------------
* add lz4 compression to rosbag (Python and C++) (`#356 <https://github.com/ros/ros_comm/issues/356>`_)
* fix rosbag record --node (`#357 <https://github.com/ros/ros_comm/issues/357>`_)
* move rosbag dox to rosbag_storage (`#389 <https://github.com/ros/ros_comm/issues/389>`_)
1.11.0 (2014-03-04)
-------------------
* use catkin_install_python() to install Python scripts (`#361 <https://github.com/ros/ros_comm/issues/361>`_)
1.10.0 (2014-02-11)
-------------------
* remove use of __connection header
1.9.54 (2014-01-27)
-------------------
* readd missing declaration of rosbag::createAdvertiseOptions (`#338 <https://github.com/ros/ros_comm/issues/338>`_)
1.9.53 (2014-01-14)
-------------------
1.9.52 (2014-01-08)
-------------------
1.9.51 (2014-01-07)
-------------------
* move several client library independent parts from ros_comm into roscpp_core, split rosbag storage specific stuff from client library usage (`#299 <https://github.com/ros/ros_comm/issues/299>`_)
* fix return value on platforms where char is unsigned.
* fix usage of boost include directories
1.9.50 (2013-10-04)
-------------------
* add chunksize option to rosbag record
1.9.49 (2013-09-16)
-------------------
1.9.48 (2013-08-21)
-------------------
* search for exported rosbag migration rules based on new package rosbag_migration_rule
1.9.47 (2013-07-03)
-------------------
1.9.46 (2013-06-18)
-------------------
* fix crash in bag migration (`#239 <https://github.com/ros/ros_comm/issues/239>`_)
1.9.45 (2013-06-06)
-------------------
* added option '--duration' to 'rosbag play' (`#121 <https://github.com/ros/ros_comm/issues/121>`_)
* fix missing newlines in rosbag error messages (`#237 <https://github.com/ros/ros_comm/issues/237>`_)
* fix flushing for tools like 'rosbag compress' (`#237 <https://github.com/ros/ros_comm/issues/237>`_)
1.9.44 (2013-03-21)
-------------------
* fix various issues on Windows (`#189 <https://github.com/ros/ros_comm/issues/189>`_)
1.9.43 (2013-03-13)
-------------------
1.9.42 (2013-03-08)
-------------------
* added option '--duration' to 'rosrun rosbag play' (`#121 <https://github.com/ros/ros_comm/issues/121>`_)
* add error message to rosbag when using same in and out file (`#171 <https://github.com/ros/ros_comm/issues/171>`_)
1.9.41 (2013-01-24)
-------------------
1.9.40 (2013-01-13)
-------------------
* fix bagsort script (`#42 <https://github.com/ros/ros_comm/issues/42>`_)
1.9.39 (2012-12-29)
-------------------
* first public release for Groovy

68
thirdparty/ros/ros_comm/tools/rosbag/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,68 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosbag)
if(NOT WIN32)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
endif()
find_package(catkin REQUIRED COMPONENTS rosbag_storage rosconsole roscpp std_srvs topic_tools xmlrpcpp)
find_package(Boost REQUIRED COMPONENTS date_time regex program_options filesystem)
find_package(BZip2 REQUIRED)
catkin_python_setup()
# Support large bags (>2GB) on 32-bit systems
add_definitions(-D_FILE_OFFSET_BITS=64)
include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
${BZIP2_INCLUDE_DIR}
)
catkin_package(
LIBRARIES rosbag
INCLUDE_DIRS include
CATKIN_DEPENDS rosbag_storage rosconsole roscpp std_srvs topic_tools xmlrpcpp)
add_library(rosbag
src/player.cpp
src/recorder.cpp
src/time_translator.cpp)
target_link_libraries(rosbag ${catkin_LIBRARIES} ${Boost_LIBRARIES}
${BZIP2_LIBRARIES}
)
add_executable(record src/record.cpp)
target_link_libraries(record rosbag)
add_executable(play src/play.cpp)
target_link_libraries(play rosbag)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h")
install(TARGETS rosbag
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(TARGETS record play
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
catkin_install_python(PROGRAMS
scripts/bag2png.py
scripts/bagsort.py
scripts/fastrebag.py
scripts/fixbag.py
scripts/fixbag_batch.py
scripts/fix_md5sums.py
scripts/fix_moved_messages.py
scripts/fix_msg_defs.py
scripts/makerule.py
scripts/savemsg.py
scripts/topic_renamer.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
if(CATKIN_ENABLE_TESTING)
catkin_add_nosetests(test)
endif()

215
thirdparty/ros/ros_comm/tools/rosbag/FORMATS vendored Normal file
View File

@@ -0,0 +1,215 @@
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
-------------
Version 1.2:
-------------
#ROSRECORD V1.2
file header
op=2
index_pos=index_1.pos
---
<empty> (padded with ' ' to 4KB)
msg_def_1
msg_1
msg_2
...
msg_N
index_1
ver=0
topic=topic
count=count
---
timestamp_1, uncompressed_byte_offset_1
timestamp_2, uncompressed_byte_offset_2
...
timestamp_N, uncompressed_byte_offset_N
index_2
...
Version 2.0:
-------------
#ROSBAG V2.0
file_header
op=3
index_pos=msg_def_1.pos # position of index
conn_count=conn_count # total number of distinct connections in the bag
chunk_count=chunk_count # total number of chunks in the bag
---
<empty> (padded with ' ' to 4KB)
chunk_1
op=5
compression=bz2 # type of compression (optional)
size=<uncompressed bytes> # size of data when uncompressed (optional)
---
conn_1i
op=7
topic=topic
id=0
---
topic=topic
message_definition=
type=
md5sum=
latching=1
callerid=caller1234
msg_1
op=2
topic=...
time=...
conn=...
---
<serialized msg>
msg_2
op=2
topic=...
time=...
conn=...
---
<serialized msg>
...
msg_N
chunk_index_1.0
op=4
ver=1
conn=conn
count=count
---
timestamp_1, uncompressed_byte_offset_1
timestamp_2, uncompressed_byte_offset_2
...
timestamp_N, uncompressed_byte_offset_N
chunk_index_1.1
op=4
ver=1
conn=conn
count=count
---
...
chunk_2
op=5
compression=bz2
size=<uncompressed bytes>
---
msg_N+1
msg_N+2
...
msg_N+M
chunk_index_2.0
op=4
ver=1
conn=conn
count=count
---
timestamp_N+1, uncompressed_byte_offset_1
timestamp_N+2, uncompressed_byte_offset_2
...
timestamp_N+M, uncompressed_byte_offset_N+M
...
conn_1
op=7
id=0
---
latching=1
callerid=caller1234
msg_def_1
op=1
topic=
type=
md5sum=
def=
---
<empty>
msg_def_2
op=1
topic=
type=
md5sum=
def=
---
<empty>
...
chunk_info_1
op=6
ver=1
chunk_pos=chunk_1.pos
start=start_stamp_1
end=end_stamp_1
count=K
---
conn_1, count_1
conn_2, count_2
...
conn_K, count_K
chunk_info_2
...
...
<eof>

49
thirdparty/ros/ros_comm/tools/rosbag/examples/write.cpp vendored Normal file
View File

@@ -0,0 +1,49 @@
// Copyright (c) 2010, 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 "rosbag/bag.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
int main()
{
// %Tag(WRITE)%
rosbag::Bag bag;
bag.open("test.bag", rosbag::bagmode::Write);
std_msgs::String str;
str.data = std::string("foo");
std_msgs::Int32 i;
i.data = 42;
bag.write("chatter", ros::Time::now(), str);
bag.write("numbers", ros::Time::now(), i);
bag.close();
// %EndTag(WRITE)%
}

48
thirdparty/ros/ros_comm/tools/rosbag/examples/write.py vendored Normal file
View File

@@ -0,0 +1,48 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2012, 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.
# %Tag(WRITE_PY)%
import rosbag
from std_msgs.msg import Int32, String
bag = rosbag.Bag('test.bag', 'w')
str = String()
str.data = 'foo'
i = Int32()
i.data = 42
bag.write('chatter', str);
bag.write('numbers', i);
bag.close();
# %EndTag(WRITE_PY)%

241
thirdparty/ros/ros_comm/tools/rosbag/include/rosbag/player.h vendored Normal file
View File

@@ -0,0 +1,241 @@
/*********************************************************************
* 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 ROSBAG_PLAYER_H
#define ROSBAG_PLAYER_H
#include <sys/stat.h>
#if !defined(_MSC_VER)
#include <termios.h>
#include <unistd.h>
#endif
#include <time.h>
#include <queue>
#include <string>
#include <ros/ros.h>
#include <ros/time.h>
#include <std_srvs/SetBool.h>
#include "rosbag/bag.h"
#include <topic_tools/shape_shifter.h>
#include "rosbag/time_translator.h"
#include "rosbag/macros.h"
namespace rosbag {
//! Helper function to create AdvertiseOptions from a MessageInstance
/*!
* param msg The Message instance for which to generate adveritse options
* param queue_size The size of the outgoing queue
* param prefix An optional prefix for all output topics
*/
ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& msg, uint32_t queue_size, const std::string& prefix = "");
ROSBAG_DECL ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size, const std::string& prefix = "");
struct ROSBAG_DECL PlayerOptions
{
PlayerOptions();
void check();
std::string prefix;
bool quiet;
bool start_paused;
bool at_once;
bool bag_time;
double bag_time_frequency;
double time_scale;
int queue_size;
ros::WallDuration advertise_sleep;
bool try_future;
bool has_time;
bool loop;
float time;
bool has_duration;
float duration;
bool keep_alive;
bool wait_for_subscribers;
std::string rate_control_topic;
float rate_control_max_delay;
ros::Duration skip_empty;
std::vector<std::string> bags;
std::vector<std::string> topics;
std::vector<std::string> pause_topics;
};
//! PRIVATE. A helper class to track relevant state for publishing time
class ROSBAG_DECL TimePublisher {
public:
/*! Create a time publisher
* A publish_frequency of < 0 indicates that time shouldn't actually be published
*/
TimePublisher();
void setPublishFrequency(double publish_frequency);
void setTimeScale(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);
/*! Get the current time */
ros::Time const& getTime() const;
/*! 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_;
};
//! PRIVATE. Player class to abstract the interface to the player
/*!
* This API is currently considered private, but will be released in the
* future after view.
*/
class ROSBAG_DECL Player
{
public:
Player(PlayerOptions const& options);
~Player();
void publish();
private:
int readCharFromStdin();
void setupTerminal();
void restoreTerminal();
void updateRateTopicTime(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event);
void doPublish(rosbag::MessageInstance const& m);
void doKeepAlive();
void printTime();
bool pauseCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res);
void processPause(const bool paused, ros::WallTime &horizon);
void waitForSubscribers() const;
private:
typedef std::map<std::string, ros::Publisher> PublisherMap;
PlayerOptions options_;
ros::NodeHandle node_handle_;
ros::ServiceServer pause_service_;
bool paused_;
bool delayed_;
bool pause_for_topics_;
bool pause_change_requested_;
bool requested_pause_state_;
ros::Subscriber rate_control_sub_;
ros::Time last_rate_control_;
ros::WallTime paused_time_;
std::vector<boost::shared_ptr<Bag> > bags_;
PublisherMap publishers_;
// Terminal
bool terminal_modified_;
#if defined(_MSC_VER)
HANDLE input_handle;
DWORD stdin_set;
#else
termios orig_flags_;
fd_set stdin_fdset_;
#endif
int maxfd_;
TimeTranslator time_translator_;
TimePublisher time_publisher_;
ros::Time start_time_;
ros::Duration bag_length_;
};
} // namespace rosbag
#endif

193
thirdparty/ros/ros_comm/tools/rosbag/include/rosbag/recorder.h vendored Normal file
View File

@@ -0,0 +1,193 @@
/*********************************************************************
* 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 ROSBAG_RECORDER_H
#define ROSBAG_RECORDER_H
#include <sys/stat.h>
#if !defined(_MSC_VER)
#include <termios.h>
#include <unistd.h>
#endif
#include <time.h>
#include <queue>
#include <string>
#include <vector>
#include <list>
#include <boost/thread/condition.hpp>
#include <boost/thread/mutex.hpp>
#include <boost/regex.hpp>
#include <ros/ros.h>
#include <ros/time.h>
#include <std_msgs/Empty.h>
#include <topic_tools/shape_shifter.h>
#include "rosbag/bag.h"
#include "rosbag/stream.h"
#include "rosbag/macros.h"
namespace rosbag {
class ROSBAG_DECL OutgoingMessage
{
public:
OutgoingMessage(std::string const& _topic, topic_tools::ShapeShifter::ConstPtr _msg, boost::shared_ptr<ros::M_string> _connection_header, ros::Time _time);
std::string topic;
topic_tools::ShapeShifter::ConstPtr msg;
boost::shared_ptr<ros::M_string> connection_header;
ros::Time time;
};
class ROSBAG_DECL OutgoingQueue
{
public:
OutgoingQueue(std::string const& _filename, std::queue<OutgoingMessage>* _queue, ros::Time _time);
std::string filename;
std::queue<OutgoingMessage>* queue;
ros::Time time;
};
struct ROSBAG_DECL RecorderOptions
{
RecorderOptions();
bool trigger;
bool record_all;
bool regex;
bool do_exclude;
bool quiet;
bool append_date;
bool snapshot;
bool verbose;
CompressionType compression;
std::string prefix;
std::string name;
boost::regex exclude_regex;
uint32_t buffer_size;
uint32_t chunk_size;
uint32_t limit;
bool split;
uint64_t max_size;
uint32_t max_splits;
ros::Duration max_duration;
std::string node;
unsigned long long min_space;
std::string min_space_str;
ros::TransportHints transport_hints;
std::vector<std::string> topics;
};
class ROSBAG_DECL Recorder
{
public:
Recorder(RecorderOptions const& options);
void doTrigger();
bool isSubscribed(std::string const& topic) const;
boost::shared_ptr<ros::Subscriber> subscribe(std::string const& topic);
int run();
private:
void printUsage();
void updateFilenames();
void startWriting();
void stopWriting();
bool checkLogging();
bool scheduledCheckDisk();
bool checkDisk();
void snapshotTrigger(std_msgs::Empty::ConstPtr trigger);
// void doQueue(topic_tools::ShapeShifter::ConstPtr msg, std::string const& topic, boost::shared_ptr<ros::Subscriber> subscriber, boost::shared_ptr<int> count);
void doQueue(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event, std::string const& topic, boost::shared_ptr<ros::Subscriber> subscriber, boost::shared_ptr<int> count);
void doRecord();
void checkNumSplits();
bool checkSize();
bool checkDuration(const ros::Time&);
void doRecordSnapshotter();
void doCheckMaster(ros::TimerEvent const& e, ros::NodeHandle& node_handle);
bool shouldSubscribeToTopic(std::string const& topic, bool from_node = false);
template<class T>
static std::string timeToStr(T ros_t);
private:
RecorderOptions options_;
Bag bag_;
std::string target_filename_;
std::string write_filename_;
std::list<std::string> current_files_;
std::set<std::string> currently_recording_; //!< set of currenly recording topics
int num_subscribers_; //!< used for book-keeping of our number of subscribers
int exit_code_; //!< eventual exit code
boost::condition_variable_any queue_condition_; //!< conditional variable for queue
boost::mutex queue_mutex_; //!< mutex for queue
std::queue<OutgoingMessage>* queue_; //!< queue for storing
uint64_t queue_size_; //!< queue size
uint64_t max_queue_size_; //!< max queue size
uint64_t split_count_; //!< split count
std::queue<OutgoingQueue> queue_queue_; //!< queue of queues to be used by the snapshot recorders
ros::Time last_buffer_warn_;
ros::Time start_time_;
bool writing_enabled_;
boost::mutex check_disk_mutex_;
ros::WallTime check_disk_next_;
ros::WallTime warn_next_;
};
} // namespace rosbag
#endif

74
thirdparty/ros/ros_comm/tools/rosbag/include/rosbag/time_translator.h vendored Normal file
View File

@@ -0,0 +1,74 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, 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 ROSBAG_TIME_TRANSLATOR_H
#define ROSBAG_TIME_TRANSLATOR_H
#include "ros/time.h"
#include "rosbag/macros.h"
namespace rosbag {
//! Helper class for translating between two times
/*!
* The time translator can be configured with a Real start time, a
* Translated start time, and a time scale.
*
* It will convert a time from a series starting at realStartTime to a
* comparable time series instead starting at translatedStartTime.
* All durations in the time-sequence as scaled by 1/(timeScale).
*
* That is, a time-sequence with time-scale 2 will finish twice as
* quickly.
*/
class ROSBAG_DECL TimeTranslator
{
public:
TimeTranslator();
void setTimeScale(double const& s);
void setRealStartTime(ros::Time const& t);
void setTranslatedStartTime(ros::Time const& t); //!< Increments the translated start time by shift. Useful for pausing.
void shift(ros::Duration const& d); //!< Increments the translated start time by shift. Useful for pausing.
ros::Time translate(ros::Time const& t);
private:
double time_scale_;
ros::Time real_start_;
ros::Time translated_start_;
};
} // namespace rosbag
#endif

9
thirdparty/ros/ros_comm/tools/rosbag/mainpage.dox vendored Normal file
View File

@@ -0,0 +1,9 @@
/**
\mainpage
\htmlinclude manifest.html
\b rosbag is a set of tools and API's for recording/writing messages to bag files and playing/reading them back.
Most code (not relying on the ROS client library) has been moved to the <a href="../../../rosbag_storage/html/c++/">rosbag_storage</a> package.
*/

46
thirdparty/ros/ros_comm/tools/rosbag/package.xml vendored Normal file
View File

@@ -0,0 +1,46 @@
<package>
<name>rosbag</name>
<version>1.12.14</version>
<description>
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>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/rosbag</url>
<author>Tim Field</author>
<author>Jeremy Leibs</author>
<author>James Bowman</author>
<buildtool_depend version_gte="0.5.78">catkin</buildtool_depend>
<build_depend>boost</build_depend>
<build_depend>cpp_common</build_depend>
<build_depend>python-imaging</build_depend>
<build_depend>rosbag_storage</build_depend>
<build_depend>rosconsole</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>roscpp_serialization</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>topic_tools</build_depend>
<build_depend>xmlrpcpp</build_depend>
<run_depend>boost</run_depend>
<run_depend>genmsg</run_depend>
<run_depend>genpy</run_depend>
<run_depend>python-rospkg</run_depend>
<run_depend>rosbag_storage</run_depend>
<run_depend>rosconsole</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>roslib</run_depend>
<run_depend>rospy</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>topic_tools</run_depend>
<run_depend>xmlrpcpp</run_depend>
<export>
<rosdoc config="${prefix}/rosdoc.yaml"/>
</export>
</package>

6
thirdparty/ros/ros_comm/tools/rosbag/rosdoc.yaml vendored 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'

69
thirdparty/ros/ros_comm/tools/rosbag/scripts/bag2png.py vendored Executable file
View File

@@ -0,0 +1,69 @@
#!/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.
import sys
import array
import Image
import rospy
import rosbag
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 sensor_msgs/Image and return a PIL image"""
if len(msg.uint8_data.data) == 0 and len(msg.int16_data.data) == 0:
return None
if msg.depth == 'uint8':
ma, image_data = msg.uint8_data, ma.data
else:
ma, image_data = msg.int16_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 rosbag.Bag(sys.argv[1]).read_messages():
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

67
thirdparty/ros/ros_comm/tools/rosbag/scripts/bagsort.py vendored Executable file
View File

@@ -0,0 +1,67 @@
#!/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.
from __future__ import print_function
import rospy
import rosbag
def sortbags(inbag, outbag):
rebag = rosbag.Bag(outbag, 'w')
try:
schedule = [(t, i) for i, (topic, msg, t) in enumerate(rosbag.Bag(inbag).read_messages(raw=True))]
schedule = [i for (t, i) in sorted(schedule)]
print(schedule)
stage = {}
for i, (topic, msg, t) in enumerate(rosbag.Bag(inbag).read_messages(raw=True)):
stage[i] = (topic, msg, t)
while (len(schedule) > 0) and (schedule[0] in stage):
(topic, msg, t) = stage[schedule[0]]
rebag.write(topic, msg, t, raw=True)
del stage[schedule[0]]
schedule = schedule[1:]
assert schedule == []
assert stage == {}
finally:
rebag.close()
if __name__ == '__main__':
import sys
if len(sys.argv) == 3:
sortbags(sys.argv[1], sys.argv[2])
else:
print("usage: bagsort.py <inbag> <outbag>")

49
thirdparty/ros/ros_comm/tools/rosbag/scripts/fastrebag.py vendored Executable file
View File

@@ -0,0 +1,49 @@
#!/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.
from __future__ import print_function
import rosbag
def fastrebag(inbag, outbag):
rebag = rosbag.Bag(outbag, 'w')
for i, (topic, msg, t) in enumerate(rosbag.Bag(inbag).read_messages(raw=True)):
rebag.write(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>')

69
thirdparty/ros/ros_comm/tools/rosbag/scripts/fix_md5sums.py vendored Executable file
View File

@@ -0,0 +1,69 @@
#!/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.
from __future__ import print_function
import os
import rospy
import rosbag
def fix_md5sums(inbags):
for b in inbags:
print('Trying to migrating file: %s' % b)
outbag = b + '.tmp'
rebag = rosbag.Bag(outbag, 'w')
try:
for i,(topic, msg, t) in enumerate(rosbag.Bag(b).read_messages(raw=True)):
rebag.write(topic, msg, t, raw=True)
rebag.close()
except rosbag.ROSBagException as e:
print(' Migration failed: %s' % str(e))
os.remove(outbag)
continue
oldnamebase = b + '.old'
oldname = oldnamebase
i = 1
while os.path.isfile(oldname):
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 ...]")

67
thirdparty/ros/ros_comm/tools/rosbag/scripts/fix_moved_messages.py vendored Executable file
View File

@@ -0,0 +1,67 @@
#!/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.
from __future__ import print_function
import rospy
import rosbag
import fileinput
def fixbags(md5file, inbag, outbag):
d = {}
for line in fileinput.input(md5file):
sp = line.split()
d[sp[1]] = [sp[0], sp[2], sp[3]]
rebag = rosbag.Bag(outbag, 'w')
for topic, msg, t in rosbag.Bag(inbag).read_messages(raw=True):
type, bytes, md5 = msg[0], msg[1], 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.write(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>')
exit(2)

79
thirdparty/ros/ros_comm/tools/rosbag/scripts/fix_msg_defs.py vendored Executable file
View File

@@ -0,0 +1,79 @@
#!/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.
from __future__ import print_function
import sys
import rosbag.migration
if __name__ == '__main__':
if len(sys.argv) != 3:
print('usage: fix_msg_defs.py <inbag> <outbag>')
exit(2)
mm = rosbag.migration.MessageMigrator()
checked = set()
migrations = []
inbag = rosbag.Bag(sys.argv[1], 'r')
outbag = rosbag.Bag(sys.argv[2], 'w')
lookup_cache = {}
#msg is: datatype, data, pytype._md5sum, bag_pos, pytype
for topic, msg, t in inbag.read_messages(raw=True):
if msg[4]._md5sum != msg[2]:
k = (msg[0], msg[2])
if k in lookup_cache:
real_msg_type = lookup_cache[k]
else:
real_msg_type = mm.lookup_type(k)
if real_msg_type != None:
print("FOUND: %s [%s] was defined in migration system\n"%(msg[0], msg[2]), file=sys.stderr)
else:
systype = roslib.message.get_message_class(msg[0])
if systype != None and systype._md5sum == msg[2]:
real_msg_type = systype
print("FOUND: %s [%s] was defined on your package path\n"%(msg[0], msg[2]), file=sys.stderr)
if real_msg_type == None:
real_msg_type = msg[4]
print("WARNING: Type [%s] with md5sum [%s] has an unknown definition.\n"%(msg[0], msg[2]), file=sys.stderr)
lookup_cache[k] = real_msg_type
outbag.write(topic, (msg[0], msg[1], msg[2], msg[3], real_msg_type), t, raw=True)
else:
outbag.write(topic, msg, t, raw=True)
inbag.close()
outbag.close()
exit(0)

54
thirdparty/ros/ros_comm/tools/rosbag/scripts/fixbag.py vendored Executable file
View File

@@ -0,0 +1,54 @@
#!/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.
from __future__ import print_function
import sys
import rosbag.migration
if __name__ == '__main__':
if len(sys.argv) < 3:
print('usage: fixbag.py <inbag> <outbag> [rulefile1, rulefile2, ...]')
exit(2)
if sys.argv[2].split('.')[-1] == 'bmr':
print('Second argument should be a bag, not a rule file.', file=sys.stderr)
exit(2)
mm = rosbag.migration.MessageMigrator(sys.argv[3:])
if not rosbag.migration.fixbag(mm, sys.argv[1], sys.argv[2]):
print('Bag could not be migrated.')
exit(1)
print('Bag migrated successfully.')
exit(0)

67
thirdparty/ros/ros_comm/tools/rosbag/scripts/fixbag_batch.py vendored Executable file
View File

@@ -0,0 +1,67 @@
#!/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.
from __future__ import print_function
import os
import sys
import rosbag.migration
def fixbag_batch(inbags):
mm = rosbag.migration.MessageMigrator()
for b in inbags:
print('Trying to migrate: %s' % b)
outbag = b + '.tmp'
if not rosbag.migration.fixbag(mm, b, outbag):
os.remove(outbag)
print(' Migration failed.')
continue
oldnamebase = b + '.old'
oldname = oldnamebase
i = 1
while os.path.isfile(oldname):
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:
fixbag_batch(sys.argv[1:])
else:
print('usage: fixbag_batch.py bag1 [bag2 bag3 ...]')
exit(2)

157
thirdparty/ros/ros_comm/tools/rosbag/scripts/makerule.py vendored Executable file
View File

@@ -0,0 +1,157 @@
#!/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.
from __future__ import print_function
import optparse
import os
import re
import sys
import rosbag.migration
import genpy.message
import genpy.dynamic
def print_trans(old, new, indent):
from_txt = '%s [%s]' % (old._type, old._md5sum)
if new is not None:
to_txt= '%s [%s]' % (new._type, new._md5sum)
else:
to_txt = 'Unknown'
print(' ' * indent + ' * From: %s' % from_txt)
print(' ' * indent + ' To: %s' % to_txt)
if __name__ == '__main__':
parser = optparse.OptionParser(usage='usage: makerule.py msg.saved [-a] output_rulefile [rulefile1, rulefile2, ...] [-n]')
parser.add_option('-a', '--append', action='store_true', dest='append', default=False)
parser.add_option('-n', '--noplugins', action='store_true', dest='noplugins', default=False)
(options, args) = parser.parse_args()
if len(args) < 2:
parser.error("Incorrect number of arguments")
rulefile = args[1]
if os.path.isfile(rulefile) and not options.append:
print("The file %s already exists. Include -a if you intend to append." % rulefile, file=sys.stderr)
exit(1)
if not os.path.isfile(rulefile) and options.append:
print("The file %s does not exist, and so -a is invalid." % rulefile, file=sys.stderr)
exit(1)
if options.append:
append_rule = [rulefile]
else:
append_rule = []
f = open(args[0])
if f is None:
print('Could not open message full definition: %s', file=sys.stderr)
sys.exit()
type_line = f.readline()
pat = re.compile(r"\[(.*)]:")
type_match = pat.match(type_line)
if type_match is None:
print("Full definition file malformed. First line should be: '[my_package/my_msg]:'", file=sys.stderr)
sys.exit()
old_type = type_match.groups()[0]
old_full_text = f.read()
f.close()
old_class = genpy.dynamic.generate_dynamic(old_type,old_full_text)[old_type]
if old_class is None:
print('Could not generate class from full definition file.', file=sys.stderr)
sys.exit()
mm = rosbag.migration.MessageMigrator(args[2:]+append_rule,not options.noplugins)
migrations = rosbag.migration.checkmessages(mm, [old_class])
if migrations == []:
print('Saved definition is up to date.')
exit(0)
print('The following migrations need to occur:')
all_rules = []
for m in migrations:
all_rules.extend(m[1])
print_trans(m[0][0].old_class, m[0][-1].new_class, 0)
if len(m[1]) > 0:
print(" %d rules missing:" % len(m[1]))
for r in m[1]:
print_trans(r.old_class, r.new_class, 1)
if rulefile is None:
print("rulefile not specified")
else:
output = ''
rules_left = mm.filter_rules_unique(all_rules)
if rules_left == []:
print("\nNo additional rule files needed to be generated. %s not created." % rulefile)
exit(0)
while rules_left != []:
extra_rules = []
for r in rules_left:
if r.new_class is None:
print("The message type %s appears to have moved. Please enter the type to migrate it to." % r.old_class._type)
new_type = raw_input('>')
new_class = genpy.message.get_message_class(new_type)
while new_class is None:
print("\'%s\' could not be found in your system. Please make sure it is built." % new_type)
new_type = raw_input('>')
new_class = genpy.message.get_message_class(new_type)
new_rule = mm.make_update_rule(r.old_class, new_class)
R = new_rule(mm, 'GENERATED.' + new_rule.__name__)
R.find_sub_paths()
new_rules = [r for r in mm.expand_rules(R.sub_rules) if r.valid == False]
extra_rules.extend(new_rules)
print('Creating the migration rule for %s requires additional missing rules:' % new_type)
for nr in new_rules:
print_trans(nr.old_class, nr.new_class,1)
output += R.get_class_def()
else:
output += r.get_class_def()
rules_left = mm.filter_rules_unique(extra_rules)
f = open(rulefile, 'a')
f.write(output)
f.close()
print("\nThe necessary rule files have been written to: %s" % rulefile)

35
thirdparty/ros/ros_comm/tools/rosbag/scripts/rosbag vendored Executable file
View File

@@ -0,0 +1,35 @@
#!/usr/bin/env python
# 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.
import rosbag
rosbag.rosbagmain()

64
thirdparty/ros/ros_comm/tools/rosbag/scripts/savemsg.py vendored Executable file
View File

@@ -0,0 +1,64 @@
#!/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.
from __future__ import print_function
import optparse
import sys
import roslib.message
import rosbag
if __name__ == '__main__':
parser = optparse.OptionParser(usage='usage: savemsg.py [-b <bagfile] type')
parser.add_option('-b', '--bagfiles', action='store', dest='bagfile', default=None, help='Save message from a bagfile rather than system definition')
(options, args) = parser.parse_args()
if len(args) < 1:
parser.error('Message type not specified.')
if options.bagfile is None:
sys_class = roslib.message.get_message_class(args[0])
if sys_class is None:
print('Could not find message %s.' % args[0], file=sys.stderr)
else:
print('[%s]:' % args[0])
print(sys_class._full_text)
else:
for topic, msg, t in rosbag.Bag(options.bagfile).read_messages(raw=True):
if msg[0] == args[0]:
print('[%s]:' % args[0])
print(msg[4]._full_text)
exit(0)
print('Could not find message %s in bag %s.' % (args[0], options.bagfile), file=sys.stderr)

50
thirdparty/ros/ros_comm/tools/rosbag/scripts/topic_renamer.py vendored Executable file
View File

@@ -0,0 +1,50 @@
#!/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.
from __future__ import print_function
import rospy
import rosbag
def rename_topic(intopic, inbag, outtopic, outbag):
rebag = rosbag.Bag(outbag, 'w')
for topic, msg, t in rosbag.Bag(inbag).read_messages(raw=True):
rebag.write(outtopic if topic == intopic else 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.py <intopic> <inbag> <outtopic> <outbag>")

13
thirdparty/ros/ros_comm/tools/rosbag/setup.py vendored Normal file
View File

@@ -0,0 +1,13 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(
packages=['rosbag'],
package_dir={'': 'src'},
scripts=['scripts/rosbag'],
requires=['genmsg', 'genpy', 'roslib', 'rospkg']
)
setup(**d)

193
thirdparty/ros/ros_comm/tools/rosbag/src/play.cpp vendored Normal file
View File

@@ -0,0 +1,193 @@
/*********************************************************************
* 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 "rosbag/player.h"
#include "boost/program_options.hpp"
namespace po = boost::program_options;
rosbag::PlayerOptions parseOptions(int argc, char** argv) {
rosbag::PlayerOptions opts;
po::options_description desc("Allowed options");
desc.add_options()
("help,h", "produce help message")
("prefix,p", po::value<std::string>()->default_value(""), "prefixes all output topics in replay")
("quiet,q", "suppress console output")
("immediate,i", "play back all messages without waiting")
("pause", "start in paused mode")
("queue", po::value<int>()->default_value(100), "use an outgoing queue of size SIZE")
("clock", "publish the clock time")
("hz", po::value<float>()->default_value(100.0f), "use a frequency of HZ when publishing clock time")
("delay,d", po::value<float>()->default_value(0.2f), "sleep SEC seconds after every advertise call (to allow subscribers to connect)")
("rate,r", po::value<float>()->default_value(1.0f), "multiply the publish rate by FACTOR")
("start,s", po::value<float>()->default_value(0.0f), "start SEC seconds into the bag files")
("duration,u", po::value<float>(), "play only SEC seconds from the bag files")
("skip-empty", po::value<float>(), "skip regions in the bag with no messages for more than SEC seconds")
("loop,l", "loop playback")
("keep-alive,k", "keep alive past end of bag (useful for publishing latched topics)")
("try-future-version", "still try to open a bag file, even if the version is not known to the player")
("topics", po::value< std::vector<std::string> >()->multitoken(), "topics to play back")
("pause-topics", po::value< std::vector<std::string> >()->multitoken(), "topics to pause playback on")
("bags", po::value< std::vector<std::string> >(), "bag files to play back from")
("wait-for-subscribers", "wait for at least one subscriber on each topic before publishing")
("rate-control-topic", po::value<std::string>(), "watch the given topic, and if the last publish was more than <rate-control-max-delay> ago, wait until the topic publishes again to continue playback")
("rate-control-max-delay", po::value<float>()->default_value(1.0f), "maximum time difference from <rate-control-topic> before pausing")
;
po::positional_options_description p;
p.add("bags", -1);
po::variables_map vm;
try
{
po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
} catch (boost::program_options::invalid_command_line_syntax& e)
{
throw ros::Exception(e.what());
} catch (boost::program_options::unknown_option& e)
{
throw ros::Exception(e.what());
}
if (vm.count("help")) {
std::cout << desc << std::endl;
exit(0);
}
if (vm.count("prefix"))
opts.prefix = vm["prefix"].as<std::string>();
if (vm.count("quiet"))
opts.quiet = true;
if (vm.count("immediate"))
opts.at_once = true;
if (vm.count("pause"))
opts.start_paused = true;
if (vm.count("queue"))
opts.queue_size = vm["queue"].as<int>();
if (vm.count("hz"))
opts.bag_time_frequency = vm["hz"].as<float>();
if (vm.count("clock"))
opts.bag_time = true;
if (vm.count("delay"))
opts.advertise_sleep = ros::WallDuration(vm["delay"].as<float>());
if (vm.count("rate"))
opts.time_scale = vm["rate"].as<float>();
if (vm.count("start"))
{
opts.time = vm["start"].as<float>();
opts.has_time = true;
}
if (vm.count("duration"))
{
opts.duration = vm["duration"].as<float>();
opts.has_duration = true;
}
if (vm.count("skip-empty"))
opts.skip_empty = ros::Duration(vm["skip-empty"].as<float>());
if (vm.count("loop"))
opts.loop = true;
if (vm.count("keep-alive"))
opts.keep_alive = true;
if (vm.count("wait-for-subscribers"))
opts.wait_for_subscribers = true;
if (vm.count("topics"))
{
std::vector<std::string> topics = vm["topics"].as< std::vector<std::string> >();
for (std::vector<std::string>::iterator i = topics.begin();
i != topics.end();
i++)
opts.topics.push_back(*i);
}
if (vm.count("pause-topics"))
{
std::vector<std::string> pause_topics = vm["pause-topics"].as< std::vector<std::string> >();
for (std::vector<std::string>::iterator i = pause_topics.begin();
i != pause_topics.end();
i++)
opts.pause_topics.push_back(*i);
}
if (vm.count("rate-control-topic"))
opts.rate_control_topic = vm["rate-control-topic"].as<std::string>();
if (vm.count("rate-control-max-delay"))
opts.rate_control_max_delay = vm["rate-control-max-delay"].as<float>();
if (vm.count("bags"))
{
std::vector<std::string> bags = vm["bags"].as< std::vector<std::string> >();
for (std::vector<std::string>::iterator i = bags.begin();
i != bags.end();
i++)
opts.bags.push_back(*i);
} else {
if (vm.count("topics") || vm.count("pause-topics"))
throw ros::Exception("When using --topics or --pause-topics, --bags "
"should be specified to list bags.");
throw ros::Exception("You must specify at least one bag to play back.");
}
return opts;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "play", ros::init_options::AnonymousName);
// Parse the command-line options
rosbag::PlayerOptions opts;
try {
opts = parseOptions(argc, argv);
}
catch (ros::Exception const& ex) {
ROS_ERROR("Error reading options: %s", ex.what());
return 1;
}
rosbag::Player player(opts);
try {
player.publish();
}
catch (std::runtime_error& e) {
ROS_FATAL("%s", e.what());
return 1;
}
return 0;
}

863
thirdparty/ros/ros_comm/tools/rosbag/src/player.cpp vendored Normal file
View File

@@ -0,0 +1,863 @@
/*********************************************************************
* 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 "rosbag/player.h"
#include "rosbag/message_instance.h"
#include "rosbag/view.h"
#if !defined(_MSC_VER)
#include <sys/select.h>
#endif
#include <boost/foreach.hpp>
#include <boost/format.hpp>
#include "rosgraph_msgs/Clock.h"
#define foreach BOOST_FOREACH
using std::map;
using std::pair;
using std::string;
using std::vector;
using boost::shared_ptr;
using ros::Exception;
namespace rosbag {
ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size, const std::string& prefix) {
ros::AdvertiseOptions opts(prefix + c->topic, queue_size, c->md5sum, c->datatype, c->msg_def);
ros::M_string::const_iterator header_iter = c->header->find("latching");
opts.latch = (header_iter != c->header->end() && header_iter->second == "1");
return opts;
}
ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& m, uint32_t queue_size, const std::string& prefix) {
return ros::AdvertiseOptions(prefix + m.getTopic(), queue_size, m.getMD5Sum(), m.getDataType(), m.getMessageDefinition());
}
// PlayerOptions
PlayerOptions::PlayerOptions() :
prefix(""),
quiet(false),
start_paused(false),
at_once(false),
bag_time(false),
bag_time_frequency(0.0),
time_scale(1.0),
queue_size(0),
advertise_sleep(0.2),
try_future(false),
has_time(false),
loop(false),
time(0.0f),
has_duration(false),
duration(0.0f),
keep_alive(false),
wait_for_subscribers(false),
rate_control_topic(""),
rate_control_max_delay(1.0f),
skip_empty(ros::DURATION_MAX)
{
}
void PlayerOptions::check() {
if (bags.size() == 0)
throw Exception("You must specify at least one bag file to play from");
if (has_duration && duration <= 0.0)
throw Exception("Invalid duration, must be > 0.0");
}
// Player
Player::Player(PlayerOptions const& options) :
options_(options),
paused_(false),
// If we were given a list of topics to pause on, then go into that mode
// by default (it can be toggled later via 't' from the keyboard).
pause_for_topics_(options_.pause_topics.size() > 0),
pause_change_requested_(false),
requested_pause_state_(false),
terminal_modified_(false)
{
ros::NodeHandle private_node_handle("~");
pause_service_ = private_node_handle.advertiseService("pause_playback", &Player::pauseCallback, this);
}
Player::~Player() {
foreach(shared_ptr<Bag> bag, bags_)
bag->close();
restoreTerminal();
}
void Player::publish() {
options_.check();
// Open all the bag files
foreach(string const& filename, options_.bags) {
ROS_INFO("Opening %s", filename.c_str());
try
{
shared_ptr<Bag> bag(boost::make_shared<Bag>());
bag->open(filename, bagmode::Read);
bags_.push_back(bag);
}
catch (BagUnindexedException ex) {
std::cerr << "Bag file " << filename << " is unindexed. Run rosbag reindex." << std::endl;
return;
}
}
setupTerminal();
if (!node_handle_.ok())
return;
if (!options_.prefix.empty())
{
ROS_INFO_STREAM("Using prefix '" << options_.prefix << "'' for topics ");
}
if (!options_.quiet)
puts("");
// Publish all messages in the bags
View full_view;
foreach(shared_ptr<Bag> bag, bags_)
full_view.addQuery(*bag);
ros::Time initial_time = full_view.getBeginTime();
initial_time += ros::Duration(options_.time);
ros::Time finish_time = ros::TIME_MAX;
if (options_.has_duration)
{
finish_time = initial_time + ros::Duration(options_.duration);
}
View view;
TopicQuery topics(options_.topics);
if (options_.topics.empty())
{
foreach(shared_ptr<Bag> bag, bags_)
view.addQuery(*bag, initial_time, finish_time);
} else {
foreach(shared_ptr<Bag> bag, bags_)
view.addQuery(*bag, topics, initial_time, finish_time);
}
if (view.size() == 0)
{
std::cerr << "No messages to play on specified topics. Exiting." << std::endl;
ros::shutdown();
return;
}
// Advertise all of our messages
foreach(const ConnectionInfo* c, view.getConnections())
{
ros::M_string::const_iterator header_iter = c->header->find("callerid");
std::string callerid = (header_iter != c->header->end() ? header_iter->second : string(""));
string callerid_topic = callerid + c->topic;
map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
if (pub_iter == publishers_.end()) {
ros::AdvertiseOptions opts = createAdvertiseOptions(c, options_.queue_size, options_.prefix);
ros::Publisher pub = node_handle_.advertise(opts);
publishers_.insert(publishers_.begin(), pair<string, ros::Publisher>(callerid_topic, pub));
pub_iter = publishers_.find(callerid_topic);
}
}
if (options_.rate_control_topic != "")
{
std::cout << "Creating rate control topic subscriber..." << std::flush;
boost::shared_ptr<ros::Subscriber> sub(boost::make_shared<ros::Subscriber>());
ros::SubscribeOptions ops;
ops.topic = options_.rate_control_topic;
ops.queue_size = 10;
ops.md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
ops.datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
const ros::MessageEvent<topic_tools::ShapeShifter const> &> >(
boost::bind(&Player::updateRateTopicTime, this, _1));
rate_control_sub_ = node_handle_.subscribe(ops);
std::cout << " done." << std::endl;
}
std::cout << "Waiting " << options_.advertise_sleep.toSec() << " seconds after advertising topics..." << std::flush;
options_.advertise_sleep.sleep();
std::cout << " done." << std::endl;
std::cout << std::endl << "Hit space to toggle paused, or 's' to step." << std::endl;
paused_ = options_.start_paused;
if (options_.wait_for_subscribers)
{
waitForSubscribers();
}
while (true) {
// Set up our time_translator and publishers
time_translator_.setTimeScale(options_.time_scale);
start_time_ = view.begin()->getTime();
time_translator_.setRealStartTime(start_time_);
bag_length_ = view.getEndTime() - view.getBeginTime();
// Set the last rate control to now, so the program doesn't start delayed.
last_rate_control_ = start_time_;
time_publisher_.setTime(start_time_);
ros::WallTime now_wt = ros::WallTime::now();
time_translator_.setTranslatedStartTime(ros::Time(now_wt.sec, now_wt.nsec));
time_publisher_.setTimeScale(options_.time_scale);
if (options_.bag_time)
time_publisher_.setPublishFrequency(options_.bag_time_frequency);
else
time_publisher_.setPublishFrequency(-1.0);
paused_time_ = now_wt;
// Call do-publish for each message
foreach(MessageInstance m, view) {
if (!node_handle_.ok())
break;
doPublish(m);
}
if (options_.keep_alive)
while (node_handle_.ok())
doKeepAlive();
if (!node_handle_.ok()) {
std::cout << std::endl;
break;
}
if (!options_.loop) {
std::cout << std::endl << "Done." << std::endl;
break;
}
}
ros::shutdown();
}
void Player::updateRateTopicTime(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event)
{
boost::shared_ptr<topic_tools::ShapeShifter const> const &ssmsg = msg_event.getConstMessage();
std::string def = ssmsg->getMessageDefinition();
size_t length = ros::serialization::serializationLength(*ssmsg);
// Check the message definition.
std::istringstream f(def);
std::string s;
bool flag = false;
while(std::getline(f, s, '\n')) {
if (!s.empty() && s.find("#") != 0) {
// Does not start with #, is not a comment.
if (s.find("Header ") == 0) {
flag = true;
}
break;
}
}
// If the header is not the first element in the message according to the definition, throw an error.
if (!flag) {
std::cout << std::endl << "WARNING: Rate control topic is bad, header is not first. MSG may be malformed." << std::endl;
return;
}
std::vector<uint8_t> buffer(length);
ros::serialization::OStream ostream(&buffer[0], length);
ros::serialization::Serializer<topic_tools::ShapeShifter>::write(ostream, *ssmsg);
// Assuming that the header is the first several bytes of the message.
//uint32_t header_sequence_id = buffer[0] | (uint32_t)buffer[1] << 8 | (uint32_t)buffer[2] << 16 | (uint32_t)buffer[3] << 24;
int32_t header_timestamp_sec = buffer[4] | (uint32_t)buffer[5] << 8 | (uint32_t)buffer[6] << 16 | (uint32_t)buffer[7] << 24;
int32_t header_timestamp_nsec = buffer[8] | (uint32_t)buffer[9] << 8 | (uint32_t)buffer[10] << 16 | (uint32_t)buffer[11] << 24;
last_rate_control_ = ros::Time(header_timestamp_sec, header_timestamp_nsec);
}
void Player::printTime()
{
if (!options_.quiet) {
ros::Time current_time = time_publisher_.getTime();
ros::Duration d = current_time - start_time_;
if (paused_)
{
printf("\r [PAUSED ] Bag Time: %13.6f Duration: %.6f / %.6f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec());
}
else if (delayed_)
{
ros::Duration time_since_rate = std::max(ros::Time::now() - last_rate_control_, ros::Duration(0));
printf("\r [DELAYED] Bag Time: %13.6f Duration: %.6f / %.6f Delay: %.2f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec(), time_since_rate.toSec());
}
else
{
printf("\r [RUNNING] Bag Time: %13.6f Duration: %.6f / %.6f \r", time_publisher_.getTime().toSec(), d.toSec(), bag_length_.toSec());
}
fflush(stdout);
}
}
bool Player::pauseCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res)
{
pause_change_requested_ = (req.data != paused_);
requested_pause_state_ = req.data;
res.success = pause_change_requested_;
if (res.success)
{
res.message = std::string("Playback is now ") + (requested_pause_state_ ? "paused" : "resumed");
}
else
{
res.message = std::string("Bag is already ") + (requested_pause_state_ ? "paused." : "running.");
}
return true;
}
void Player::processPause(const bool paused, ros::WallTime &horizon)
{
paused_ = paused;
if (paused_)
{
paused_time_ = ros::WallTime::now();
}
else
{
// Make sure time doesn't shift after leaving pause.
ros::WallDuration shift = ros::WallTime::now() - paused_time_;
paused_time_ = ros::WallTime::now();
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
}
}
void Player::waitForSubscribers() const
{
bool all_topics_subscribed = false;
std::cout << "Waiting for subscribers." << std::endl;
while (!all_topics_subscribed) {
all_topics_subscribed = true;
foreach(const PublisherMap::value_type& pub, publishers_) {
all_topics_subscribed &= pub.second.getNumSubscribers() > 0;
}
ros::Duration(0.1).sleep();
}
std::cout << "Finished waiting for subscribers." << std::endl;
}
void Player::doPublish(MessageInstance const& m) {
string const& topic = m.getTopic();
ros::Time const& time = m.getTime();
string callerid = m.getCallerId();
ros::Time translated = time_translator_.translate(time);
ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
time_publisher_.setHorizon(time);
time_publisher_.setWCHorizon(horizon);
string callerid_topic = callerid + topic;
map<string, ros::Publisher>::iterator pub_iter = publishers_.find(callerid_topic);
ROS_ASSERT(pub_iter != publishers_.end());
// Update subscribers.
ros::spinOnce();
// If immediate specified, play immediately
if (options_.at_once) {
time_publisher_.stepClock();
pub_iter->second.publish(m);
printTime();
return;
}
// If skip_empty is specified, skip this region and shift.
if (time - time_publisher_.getTime() > options_.skip_empty)
{
time_publisher_.stepClock();
ros::WallDuration shift = ros::WallTime::now() - horizon ;
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
(pub_iter->second).publish(m);
printTime();
return;
}
if (pause_for_topics_)
{
for (std::vector<std::string>::iterator i = options_.pause_topics.begin();
i != options_.pause_topics.end();
++i)
{
if (topic == *i)
{
paused_ = true;
paused_time_ = ros::WallTime::now();
}
}
}
// Check if the rate control topic has posted recently enough to continue, or if a delay is needed.
// Delayed is separated from paused to allow more verbose printing.
if (rate_control_sub_ != NULL) {
if ((time_publisher_.getTime() - last_rate_control_).toSec() > options_.rate_control_max_delay) {
delayed_ = true;
paused_time_ = ros::WallTime::now();
}
}
while ((paused_ || delayed_ || !time_publisher_.horizonReached()) && node_handle_.ok())
{
bool charsleftorpaused = true;
while (charsleftorpaused && node_handle_.ok())
{
ros::spinOnce();
if (pause_change_requested_)
{
processPause(requested_pause_state_, horizon);
pause_change_requested_ = false;
}
switch (readCharFromStdin()){
case ' ':
processPause(!paused_, horizon);
break;
case 's':
if (paused_) {
time_publisher_.stepClock();
ros::WallDuration shift = ros::WallTime::now() - horizon ;
paused_time_ = ros::WallTime::now();
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
(pub_iter->second).publish(m);
printTime();
return;
}
break;
case 't':
pause_for_topics_ = !pause_for_topics_;
break;
case EOF:
if (paused_)
{
printTime();
time_publisher_.runStalledClock(ros::WallDuration(.1));
ros::spinOnce();
}
else if (delayed_)
{
printTime();
time_publisher_.runStalledClock(ros::WallDuration(.1));
ros::spinOnce();
// You need to check the rate here too.
if(rate_control_sub_ == NULL || (time_publisher_.getTime() - last_rate_control_).toSec() <= options_.rate_control_max_delay) {
delayed_ = false;
// Make sure time doesn't shift after leaving delay.
ros::WallDuration shift = ros::WallTime::now() - paused_time_;
paused_time_ = ros::WallTime::now();
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
}
}
else
charsleftorpaused = false;
}
}
printTime();
time_publisher_.runClock(ros::WallDuration(.1));
ros::spinOnce();
}
pub_iter->second.publish(m);
}
void Player::doKeepAlive() {
//Keep pushing ourself out in 10-sec increments (avoids fancy math dealing with the end of time)
ros::Time const& time = time_publisher_.getTime() + ros::Duration(10.0);
ros::Time translated = time_translator_.translate(time);
ros::WallTime horizon = ros::WallTime(translated.sec, translated.nsec);
time_publisher_.setHorizon(time);
time_publisher_.setWCHorizon(horizon);
if (options_.at_once) {
return;
}
// If we're done and just staying alive, don't watch the rate control topic. We aren't publishing anyway.
delayed_ = false;
while ((paused_ || !time_publisher_.horizonReached()) && node_handle_.ok())
{
bool charsleftorpaused = true;
while (charsleftorpaused && node_handle_.ok())
{
switch (readCharFromStdin()){
case ' ':
paused_ = !paused_;
if (paused_) {
paused_time_ = ros::WallTime::now();
}
else
{
// Make sure time doesn't shift after leaving pause.
ros::WallDuration shift = ros::WallTime::now() - paused_time_;
paused_time_ = ros::WallTime::now();
time_translator_.shift(ros::Duration(shift.sec, shift.nsec));
horizon += shift;
time_publisher_.setWCHorizon(horizon);
}
break;
case EOF:
if (paused_)
{
printTime();
time_publisher_.runStalledClock(ros::WallDuration(.1));
ros::spinOnce();
}
else
charsleftorpaused = false;
}
}
printTime();
time_publisher_.runClock(ros::WallDuration(.1));
ros::spinOnce();
}
}
void Player::setupTerminal() {
if (terminal_modified_)
return;
#if defined(_MSC_VER)
input_handle = GetStdHandle(STD_INPUT_HANDLE);
if (input_handle == INVALID_HANDLE_VALUE)
{
std::cout << "Failed to set up standard input handle." << std::endl;
return;
}
if (! GetConsoleMode(input_handle, &stdin_set) )
{
std::cout << "Failed to save the console mode." << std::endl;
return;
}
// don't actually need anything but the default, alternatively try this
//DWORD event_mode = ENABLE_WINDOW_INPUT | ENABLE_MOUSE_INPUT;
//if (! SetConsoleMode(input_handle, event_mode) )
//{
// std::cout << "Failed to set the console mode." << std::endl;
// return;
//}
terminal_modified_ = true;
#else
const int fd = fileno(stdin);
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;
terminal_modified_ = true;
#endif
}
void Player::restoreTerminal() {
if (!terminal_modified_)
return;
#if defined(_MSC_VER)
SetConsoleMode(input_handle, stdin_set);
#else
const int fd = fileno(stdin);
tcsetattr(fd, TCSANOW, &orig_flags_);
#endif
terminal_modified_ = false;
}
int Player::readCharFromStdin() {
#ifdef __APPLE__
fd_set testfd;
FD_COPY(&stdin_fdset_, &testfd);
#elif !defined(_MSC_VER)
fd_set testfd = stdin_fdset_;
#endif
#if defined(_MSC_VER)
DWORD events = 0;
INPUT_RECORD input_record[1];
DWORD input_size = 1;
BOOL b = GetNumberOfConsoleInputEvents(input_handle, &events);
if (b && events > 0)
{
b = ReadConsoleInput(input_handle, input_record, input_size, &events);
if (b)
{
for (unsigned int i = 0; i < events; ++i)
{
if (input_record[i].EventType & KEY_EVENT & input_record[i].Event.KeyEvent.bKeyDown)
{
CHAR ch = input_record[i].Event.KeyEvent.uChar.AsciiChar;
return ch;
}
}
}
}
return EOF;
#else
timeval tv;
tv.tv_sec = 0;
tv.tv_usec = 0;
if (select(maxfd_, &testfd, NULL, NULL, &tv) <= 0)
return EOF;
return getc(stdin);
#endif
}
TimePublisher::TimePublisher() : time_scale_(1.0)
{
setPublishFrequency(-1.0);
time_pub_ = node_handle_.advertise<rosgraph_msgs::Clock>("clock",1);
}
void TimePublisher::setPublishFrequency(double publish_frequency)
{
publish_frequency_ = publish_frequency;
do_publish_ = (publish_frequency > 0.0);
wall_step_.fromSec(1.0 / publish_frequency);
}
void TimePublisher::setTimeScale(double time_scale)
{
time_scale_ = time_scale;
}
void TimePublisher::setHorizon(const ros::Time& horizon)
{
horizon_ = horizon;
}
void TimePublisher::setWCHorizon(const ros::WallTime& horizon)
{
wc_horizon_ = horizon;
}
void TimePublisher::setTime(const ros::Time& time)
{
current_ = time;
}
ros::Time const& TimePublisher::getTime() const
{
return current_;
}
void TimePublisher::runClock(const ros::WallDuration& duration)
{
if (do_publish_)
{
rosgraph_msgs::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 t = ros::WallTime::now();
ros::WallDuration leftHorizonWC = wc_horizon_ - t;
ros::Duration d(leftHorizonWC.sec, leftHorizonWC.nsec);
d *= time_scale_;
current_ = horizon_ - d;
if (current_ >= horizon_)
current_ = horizon_;
ros::WallTime target = ros::WallTime::now() + duration;
if (target > wc_horizon_)
target = wc_horizon_;
ros::WallTime::sleepUntil(target);
}
}
void TimePublisher::stepClock()
{
if (do_publish_)
{
current_ = horizon_;
rosgraph_msgs::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 TimePublisher::runStalledClock(const ros::WallDuration& duration)
{
if (do_publish_)
{
rosgraph_msgs::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 TimePublisher::horizonReached()
{
return ros::WallTime::now() > wc_horizon_;
}
} // namespace rosbag

298
thirdparty/ros/ros_comm/tools/rosbag/src/record.cpp vendored Normal file
View File

@@ -0,0 +1,298 @@
/*********************************************************************
* 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 "rosbag/recorder.h"
#include "rosbag/exceptions.h"
#include "boost/program_options.hpp"
#include <string>
#include <sstream>
namespace po = boost::program_options;
//! Parse the command-line arguments for recorder options
rosbag::RecorderOptions parseOptions(int argc, char** argv) {
rosbag::RecorderOptions opts;
po::options_description desc("Allowed options");
desc.add_options()
("help,h", "produce help message")
("all,a", "record all topics")
("regex,e", "match topics using regular expressions")
("exclude,x", po::value<std::string>(), "exclude topics matching regular expressions")
("quiet,q", "suppress console output")
("output-prefix,o", po::value<std::string>(), "prepend PREFIX to beginning of bag name")
("output-name,O", po::value<std::string>(), "record bagnamed NAME.bag")
("buffsize,b", po::value<int>()->default_value(256), "Use an internal buffer of SIZE MB (Default: 256)")
("chunksize", po::value<int>()->default_value(768), "Set chunk size of message data, in KB (Default: 768. Advanced)")
("limit,l", po::value<int>()->default_value(0), "Only record NUM messages on each topic")
("min-space,L", po::value<std::string>()->default_value("1G"), "Minimum allowed space on recording device (use G,M,k multipliers)")
("bz2,j", "use BZ2 compression")
("lz4", "use LZ4 compression")
("split", po::value<int>()->implicit_value(0), "Split the bag file and continue recording when maximum size or maximum duration reached.")
("max-splits", po::value<int>(), "Keep a maximum of N bag files, when reaching the maximum erase the oldest one to keep a constant number of files.")
("topic", po::value< std::vector<std::string> >(), "topic to record")
("size", po::value<uint64_t>(), "The maximum size of the bag to record in MB.")
("duration", po::value<std::string>(), "Record a bag of maximum duration in seconds, unless 'm', or 'h' is appended.")
("node", po::value<std::string>(), "Record all topics subscribed to by a specific node.")
("tcpnodelay", "Use the TCP_NODELAY transport hint when subscribing to topics.")
("udp", "Use the UDP transport hint when subscribing to topics.");
po::positional_options_description p;
p.add("topic", -1);
po::variables_map vm;
try
{
po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm);
} catch (boost::program_options::invalid_command_line_syntax& e)
{
throw ros::Exception(e.what());
} catch (boost::program_options::unknown_option& e)
{
throw ros::Exception(e.what());
}
if (vm.count("help")) {
std::cout << desc << std::endl;
exit(0);
}
if (vm.count("all"))
opts.record_all = true;
if (vm.count("regex"))
opts.regex = true;
if (vm.count("exclude"))
{
opts.do_exclude = true;
opts.exclude_regex = vm["exclude"].as<std::string>();
}
if (vm.count("quiet"))
opts.quiet = true;
if (vm.count("output-prefix"))
{
opts.prefix = vm["output-prefix"].as<std::string>();
opts.append_date = true;
}
if (vm.count("output-name"))
{
opts.prefix = vm["output-name"].as<std::string>();
opts.append_date = false;
}
if (vm.count("split"))
{
opts.split = true;
int S = vm["split"].as<int>();
if (S != 0)
{
ROS_WARN("Use of \"--split <MAX_SIZE>\" has been deprecated. Please use --split --size <MAX_SIZE> or --split --duration <MAX_DURATION>");
if (S < 0)
throw ros::Exception("Split size must be 0 or positive");
opts.max_size = 1048576 * S;
}
}
if(vm.count("max-splits"))
{
if(!opts.split)
{
ROS_WARN("--max-splits is ignored without --split");
}
else
{
opts.max_splits = vm["max-splits"].as<int>();
}
}
if (vm.count("buffsize"))
{
int m = vm["buffsize"].as<int>();
if (m < 0)
throw ros::Exception("Buffer size must be 0 or positive");
opts.buffer_size = 1048576 * m;
}
if (vm.count("chunksize"))
{
int chnk_sz = vm["chunksize"].as<int>();
if (chnk_sz < 0)
throw ros::Exception("Chunk size must be 0 or positive");
opts.chunk_size = 1024 * chnk_sz;
}
if (vm.count("limit"))
{
opts.limit = vm["limit"].as<int>();
}
if (vm.count("min-space"))
{
std::string ms = vm["min-space"].as<std::string>();
long long int value = 1073741824ull;
char mul = 0;
// Sane default values, just in case
opts.min_space_str = "1G";
opts.min_space = value;
if (sscanf(ms.c_str(), " %lld%c", &value, &mul) > 0) {
opts.min_space_str = ms;
switch (mul) {
case 'G':
case 'g':
opts.min_space = value * 1073741824ull;
break;
case 'M':
case 'm':
opts.min_space = value * 1048576ull;
break;
case 'K':
case 'k':
opts.min_space = value * 1024ull;
break;
default:
opts.min_space = value;
break;
}
}
ROS_DEBUG("Rosbag using minimum space of %lld bytes, or %s", opts.min_space, opts.min_space_str.c_str());
}
if (vm.count("bz2") && vm.count("lz4"))
{
throw ros::Exception("Can only use one type of compression");
}
if (vm.count("bz2"))
{
opts.compression = rosbag::compression::BZ2;
}
if (vm.count("lz4"))
{
opts.compression = rosbag::compression::LZ4;
}
if (vm.count("duration"))
{
std::string duration_str = vm["duration"].as<std::string>();
double duration;
double multiplier = 1.0;
std::string unit("");
std::istringstream iss(duration_str);
if ((iss >> duration).fail())
throw ros::Exception("Duration must start with a floating point number.");
if ( (!iss.eof() && ((iss >> unit).fail())) )
{
throw ros::Exception("Duration unit must be s, m, or h");
}
if (unit == std::string(""))
multiplier = 1.0;
else if (unit == std::string("s"))
multiplier = 1.0;
else if (unit == std::string("m"))
multiplier = 60.0;
else if (unit == std::string("h"))
multiplier = 3600.0;
else
throw ros::Exception("Duration unit must be s, m, or h");
opts.max_duration = ros::Duration(duration * multiplier);
if (opts.max_duration <= ros::Duration(0))
throw ros::Exception("Duration must be positive.");
}
if (vm.count("size"))
{
opts.max_size = vm["size"].as<uint64_t>() * 1048576;
if (opts.max_size <= 0)
throw ros::Exception("Split size must be 0 or positive");
}
if (vm.count("node"))
{
opts.node = vm["node"].as<std::string>();
std::cout << "Recording from: " << opts.node << std::endl;
}
if (vm.count("tcpnodelay"))
{
opts.transport_hints.tcpNoDelay();
}
if (vm.count("udp"))
{
opts.transport_hints.udp();
}
// Every non-option argument is assumed to be a topic
if (vm.count("topic"))
{
std::vector<std::string> bags = vm["topic"].as< std::vector<std::string> >();
std::sort(bags.begin(), bags.end());
bags.erase(std::unique(bags.begin(), bags.end()), bags.end());
for (std::vector<std::string>::iterator i = bags.begin();
i != bags.end();
i++)
opts.topics.push_back(*i);
}
// check that argument combinations make sense
if(opts.exclude_regex.size() > 0 &&
!(opts.record_all || opts.regex)) {
fprintf(stderr, "Warning: Exclusion regex given, but no topics to subscribe to.\n"
"Adding implicit 'record all'.");
opts.record_all = true;
}
return opts;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "record", ros::init_options::AnonymousName);
// Parse the command-line options
rosbag::RecorderOptions opts;
try {
opts = parseOptions(argc, argv);
}
catch (ros::Exception const& ex) {
ROS_ERROR("Error reading options: %s", ex.what());
return 1;
}
catch(boost::regex_error const& ex) {
ROS_ERROR("Error reading options: %s\n", ex.what());
return 1;
}
// Run the recorder
rosbag::Recorder recorder(opts);
int result = recorder.run();
return result;
}

706
thirdparty/ros/ros_comm/tools/rosbag/src/recorder.cpp vendored Normal file
View File

@@ -0,0 +1,706 @@
/*********************************************************************
* 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 "rosbag/recorder.h"
#include <sys/stat.h>
#include <boost/filesystem.hpp>
// Boost filesystem v3 is default in 1.46.0 and above
// Fallback to original posix code (*nix only) if this is not true
#if BOOST_FILESYSTEM_VERSION < 3
#include <sys/statvfs.h>
#endif
#include <time.h>
#include <queue>
#include <set>
#include <sstream>
#include <string>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/regex.hpp>
#include <boost/thread.hpp>
#include <boost/thread/xtime.hpp>
#include <boost/date_time/local_time/local_time.hpp>
#include <ros/ros.h>
#include <topic_tools/shape_shifter.h>
#include "ros/network.h"
#include "ros/xmlrpc_manager.h"
#include "xmlrpcpp/XmlRpc.h"
#define foreach BOOST_FOREACH
using std::cout;
using std::endl;
using std::set;
using std::string;
using std::vector;
using boost::shared_ptr;
using ros::Time;
namespace rosbag {
// OutgoingMessage
OutgoingMessage::OutgoingMessage(string const& _topic, topic_tools::ShapeShifter::ConstPtr _msg, boost::shared_ptr<ros::M_string> _connection_header, Time _time) :
topic(_topic), msg(_msg), connection_header(_connection_header), time(_time)
{
}
// OutgoingQueue
OutgoingQueue::OutgoingQueue(string const& _filename, std::queue<OutgoingMessage>* _queue, Time _time) :
filename(_filename), queue(_queue), time(_time)
{
}
// RecorderOptions
RecorderOptions::RecorderOptions() :
trigger(false),
record_all(false),
regex(false),
do_exclude(false),
quiet(false),
append_date(true),
snapshot(false),
verbose(false),
compression(compression::Uncompressed),
prefix(""),
name(""),
exclude_regex(),
buffer_size(1048576 * 256),
chunk_size(1024 * 768),
limit(0),
split(false),
max_size(0),
max_splits(0),
max_duration(-1.0),
node(""),
min_space(1024 * 1024 * 1024),
min_space_str("1G")
{
}
// Recorder
Recorder::Recorder(RecorderOptions const& options) :
options_(options),
num_subscribers_(0),
exit_code_(0),
queue_size_(0),
split_count_(0),
writing_enabled_(true)
{
}
int Recorder::run() {
if (options_.trigger) {
doTrigger();
return 0;
}
if (options_.topics.size() == 0) {
// Make sure limit is not specified with automatic topic subscription
if (options_.limit > 0) {
fprintf(stderr, "Specifing a count is not valid with automatic topic subscription.\n");
return 1;
}
// Make sure topics are specified
if (!options_.record_all && (options_.node == std::string(""))) {
fprintf(stderr, "No topics specified.\n");
return 1;
}
}
ros::NodeHandle nh;
if (!nh.ok())
return 0;
last_buffer_warn_ = Time();
queue_ = new std::queue<OutgoingMessage>;
// Subscribe to each topic
if (!options_.regex) {
foreach(string const& topic, options_.topics)
subscribe(topic);
}
if (!ros::Time::waitForValid(ros::WallDuration(2.0)))
ROS_WARN("/use_sim_time set to true and no clock published. Still waiting for valid time...");
ros::Time::waitForValid();
start_time_ = ros::Time::now();
// Don't bother doing anything if we never got a valid time
if (!nh.ok())
return 0;
ros::Subscriber trigger_sub;
// Spin up a thread for writing to the file
boost::thread record_thread;
if (options_.snapshot)
{
record_thread = boost::thread(boost::bind(&Recorder::doRecordSnapshotter, this));
// Subscribe to the snapshot trigger
trigger_sub = nh.subscribe<std_msgs::Empty>("snapshot_trigger", 100, boost::bind(&Recorder::snapshotTrigger, this, _1));
}
else
record_thread = boost::thread(boost::bind(&Recorder::doRecord, this));
ros::Timer check_master_timer;
if (options_.record_all || options_.regex || (options_.node != std::string("")))
{
// check for master first
doCheckMaster(ros::TimerEvent(), nh);
check_master_timer = nh.createTimer(ros::Duration(1.0), boost::bind(&Recorder::doCheckMaster, this, _1, boost::ref(nh)));
}
ros::MultiThreadedSpinner s(10);
ros::spin(s);
queue_condition_.notify_all();
record_thread.join();
delete queue_;
return exit_code_;
}
shared_ptr<ros::Subscriber> Recorder::subscribe(string const& topic) {
ROS_INFO("Subscribing to %s", topic.c_str());
ros::NodeHandle nh;
shared_ptr<int> count(boost::make_shared<int>(options_.limit));
shared_ptr<ros::Subscriber> sub(boost::make_shared<ros::Subscriber>());
ros::SubscribeOptions ops;
ops.topic = topic;
ops.queue_size = 100;
ops.md5sum = ros::message_traits::md5sum<topic_tools::ShapeShifter>();
ops.datatype = ros::message_traits::datatype<topic_tools::ShapeShifter>();
ops.helper = boost::make_shared<ros::SubscriptionCallbackHelperT<
const ros::MessageEvent<topic_tools::ShapeShifter const> &> >(
boost::bind(&Recorder::doQueue, this, _1, topic, sub, count));
ops.transport_hints = options_.transport_hints;
*sub = nh.subscribe(ops);
currently_recording_.insert(topic);
num_subscribers_++;
return sub;
}
bool Recorder::isSubscribed(string const& topic) const {
return currently_recording_.find(topic) != currently_recording_.end();
}
bool Recorder::shouldSubscribeToTopic(std::string const& topic, bool from_node) {
// ignore already known topics
if (isSubscribed(topic)) {
return false;
}
// subtract exclusion regex, if any
if(options_.do_exclude && boost::regex_match(topic, options_.exclude_regex)) {
return false;
}
if(options_.record_all || from_node) {
return true;
}
if (options_.regex) {
// Treat the topics as regular expressions
foreach(string const& regex_str, options_.topics) {
boost::regex e(regex_str);
boost::smatch what;
if (boost::regex_match(topic, what, e, boost::match_extra))
return true;
}
}
else {
foreach(string const& t, options_.topics)
if (t == topic)
return true;
}
return false;
}
template<class T>
std::string Recorder::timeToStr(T ros_t)
{
(void)ros_t;
std::stringstream msg;
const boost::posix_time::ptime now=
boost::posix_time::second_clock::local_time();
boost::posix_time::time_facet *const f=
new boost::posix_time::time_facet("%Y-%m-%d-%H-%M-%S");
msg.imbue(std::locale(msg.getloc(),f));
msg << now;
return msg.str();
}
//! Callback to be invoked to save messages into a queue
void Recorder::doQueue(const ros::MessageEvent<topic_tools::ShapeShifter const>& msg_event, string const& topic, shared_ptr<ros::Subscriber> subscriber, shared_ptr<int> count) {
//void Recorder::doQueue(topic_tools::ShapeShifter::ConstPtr msg, string const& topic, shared_ptr<ros::Subscriber> subscriber, shared_ptr<int> count) {
Time rectime = Time::now();
if (options_.verbose)
cout << "Received message on topic " << subscriber->getTopic() << endl;
OutgoingMessage out(topic, msg_event.getMessage(), msg_event.getConnectionHeaderPtr(), rectime);
{
boost::mutex::scoped_lock lock(queue_mutex_);
queue_->push(out);
queue_size_ += out.msg->size();
// Check to see if buffer has been exceeded
while (options_.buffer_size > 0 && queue_size_ > options_.buffer_size) {
OutgoingMessage drop = queue_->front();
queue_->pop();
queue_size_ -= drop.msg->size();
if (!options_.snapshot) {
Time now = Time::now();
if (now > last_buffer_warn_ + ros::Duration(5.0)) {
ROS_WARN("rosbag record buffer exceeded. Dropping oldest queued message.");
last_buffer_warn_ = now;
}
}
}
}
if (!options_.snapshot)
queue_condition_.notify_all();
// If we are book-keeping count, decrement and possibly shutdown
if ((*count) > 0) {
(*count)--;
if ((*count) == 0) {
subscriber->shutdown();
num_subscribers_--;
if (num_subscribers_ == 0)
ros::shutdown();
}
}
}
void Recorder::updateFilenames() {
vector<string> parts;
std::string prefix = options_.prefix;
size_t ind = prefix.rfind(".bag");
if (ind != std::string::npos && ind == prefix.size() - 4)
{
prefix.erase(ind);
}
if (prefix.length() > 0)
parts.push_back(prefix);
if (options_.append_date)
parts.push_back(timeToStr(ros::WallTime::now()));
if (options_.split)
parts.push_back(boost::lexical_cast<string>(split_count_));
if (parts.size() == 0)
{
throw BagException("Bag filename is empty (neither of these was specified: prefix, append_date, split)");
}
target_filename_ = parts[0];
for (unsigned int i = 1; i < parts.size(); i++)
target_filename_ += string("_") + parts[i];
target_filename_ += string(".bag");
write_filename_ = target_filename_ + string(".active");
}
//! Callback to be invoked to actually do the recording
void Recorder::snapshotTrigger(std_msgs::Empty::ConstPtr trigger) {
(void)trigger;
updateFilenames();
ROS_INFO("Triggered snapshot recording with name %s.", target_filename_.c_str());
{
boost::mutex::scoped_lock lock(queue_mutex_);
queue_queue_.push(OutgoingQueue(target_filename_, queue_, Time::now()));
queue_ = new std::queue<OutgoingMessage>;
queue_size_ = 0;
}
queue_condition_.notify_all();
}
void Recorder::startWriting() {
bag_.setCompression(options_.compression);
bag_.setChunkThreshold(options_.chunk_size);
updateFilenames();
try {
bag_.open(write_filename_, bagmode::Write);
}
catch (rosbag::BagException e) {
ROS_ERROR("Error writing: %s", e.what());
exit_code_ = 1;
ros::shutdown();
}
ROS_INFO("Recording to %s.", target_filename_.c_str());
}
void Recorder::stopWriting() {
ROS_INFO("Closing %s.", target_filename_.c_str());
bag_.close();
rename(write_filename_.c_str(), target_filename_.c_str());
}
void Recorder::checkNumSplits()
{
if(options_.max_splits>0)
{
current_files_.push_back(target_filename_);
if(current_files_.size()>options_.max_splits)
{
int err = unlink(current_files_.front().c_str());
if(err != 0)
{
ROS_ERROR("Unable to remove %s: %s", current_files_.front().c_str(), strerror(errno));
}
current_files_.pop_front();
}
}
}
bool Recorder::checkSize()
{
if (options_.max_size > 0)
{
if (bag_.getSize() > options_.max_size)
{
if (options_.split)
{
stopWriting();
split_count_++;
checkNumSplits();
startWriting();
} else {
ros::shutdown();
return true;
}
}
}
return false;
}
bool Recorder::checkDuration(const ros::Time& t)
{
if (options_.max_duration > ros::Duration(0))
{
if (t - start_time_ > options_.max_duration)
{
if (options_.split)
{
while (start_time_ + options_.max_duration < t)
{
stopWriting();
split_count_++;
checkNumSplits();
start_time_ += options_.max_duration;
startWriting();
}
} else {
ros::shutdown();
return true;
}
}
}
return false;
}
//! Thread that actually does writing to file.
void Recorder::doRecord() {
// Open bag file for writing
startWriting();
// Schedule the disk space check
warn_next_ = ros::WallTime();
checkDisk();
check_disk_next_ = ros::WallTime::now() + ros::WallDuration().fromSec(20.0);
// Technically the 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.
ros::NodeHandle nh;
while (nh.ok() || !queue_->empty()) {
boost::unique_lock<boost::mutex> lock(queue_mutex_);
bool finished = false;
while (queue_->empty()) {
if (!nh.ok()) {
lock.release()->unlock();
finished = true;
break;
}
boost::xtime xt;
#if BOOST_VERSION >= 105000
boost::xtime_get(&xt, boost::TIME_UTC_);
#else
boost::xtime_get(&xt, boost::TIME_UTC);
#endif
xt.nsec += 250000000;
queue_condition_.timed_wait(lock, xt);
if (checkDuration(ros::Time::now()))
{
finished = true;
break;
}
}
if (finished)
break;
OutgoingMessage out = queue_->front();
queue_->pop();
queue_size_ -= out.msg->size();
lock.release()->unlock();
if (checkSize())
break;
if (checkDuration(out.time))
break;
if (scheduledCheckDisk() && checkLogging())
bag_.write(out.topic, out.time, *out.msg, out.connection_header);
}
stopWriting();
}
void Recorder::doRecordSnapshotter() {
ros::NodeHandle nh;
while (nh.ok() || !queue_queue_.empty()) {
boost::unique_lock<boost::mutex> lock(queue_mutex_);
while (queue_queue_.empty()) {
if (!nh.ok())
return;
queue_condition_.wait(lock);
}
OutgoingQueue out_queue = queue_queue_.front();
queue_queue_.pop();
lock.release()->unlock();
string target_filename = out_queue.filename;
string write_filename = target_filename + string(".active");
try {
bag_.open(write_filename, bagmode::Write);
}
catch (rosbag::BagException ex) {
ROS_ERROR("Error writing: %s", ex.what());
return;
}
while (!out_queue.queue->empty()) {
OutgoingMessage out = out_queue.queue->front();
out_queue.queue->pop();
bag_.write(out.topic, out.time, *out.msg);
}
stopWriting();
}
}
void Recorder::doCheckMaster(ros::TimerEvent const& e, ros::NodeHandle& node_handle) {
(void)e;
(void)node_handle;
ros::master::V_TopicInfo topics;
if (ros::master::getTopics(topics)) {
foreach(ros::master::TopicInfo const& t, topics) {
if (shouldSubscribeToTopic(t.name))
subscribe(t.name);
}
}
if (options_.node != std::string(""))
{
XmlRpc::XmlRpcValue req;
req[0] = ros::this_node::getName();
req[1] = options_.node;
XmlRpc::XmlRpcValue resp;
XmlRpc::XmlRpcValue payload;
if (ros::master::execute("lookupNode", req, resp, payload, true))
{
std::string peer_host;
uint32_t peer_port;
if (!ros::network::splitURI(static_cast<std::string>(resp[2]), peer_host, peer_port))
{
ROS_ERROR("Bad xml-rpc URI trying to inspect node at: [%s]", static_cast<std::string>(resp[2]).c_str());
} else {
XmlRpc::XmlRpcClient c(peer_host.c_str(), peer_port, "/");
XmlRpc::XmlRpcValue req2;
XmlRpc::XmlRpcValue resp2;
req2[0] = ros::this_node::getName();
c.execute("getSubscriptions", req2, resp2);
if (!c.isFault() && resp2.valid() && resp2.size() > 0 && static_cast<int>(resp2[0]) == 1)
{
for(int i = 0; i < resp2[2].size(); i++)
{
if (shouldSubscribeToTopic(resp2[2][i][0], true))
subscribe(resp2[2][i][0]);
}
} else {
ROS_ERROR("Node at: [%s] failed to return subscriptions.", static_cast<std::string>(resp[2]).c_str());
}
}
}
}
}
void Recorder::doTrigger() {
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::Empty>("snapshot_trigger", 1, true);
pub.publish(std_msgs::Empty());
ros::Timer terminate_timer = nh.createTimer(ros::Duration(1.0), boost::bind(&ros::shutdown));
ros::spin();
}
bool Recorder::scheduledCheckDisk() {
boost::mutex::scoped_lock lock(check_disk_mutex_);
if (ros::WallTime::now() < check_disk_next_)
return true;
check_disk_next_ += ros::WallDuration().fromSec(20.0);
return checkDisk();
}
bool Recorder::checkDisk() {
#if BOOST_FILESYSTEM_VERSION < 3
struct statvfs fiData;
if ((statvfs(bag_.getFileName().c_str(), &fiData)) < 0)
{
ROS_WARN("Failed to check filesystem stats.");
return true;
}
unsigned long long free_space = 0;
free_space = (unsigned long long) (fiData.f_bsize) * (unsigned long long) (fiData.f_bavail);
if (free_space < options_.min_space)
{
ROS_ERROR("Less than %s of space free on disk with %s. Disabling recording.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
writing_enabled_ = false;
return false;
}
else if (free_space < 5 * options_.min_space)
{
ROS_WARN("Less than 5 x %s of space free on disk with %s.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
}
else
{
writing_enabled_ = true;
}
#else
boost::filesystem::path p(boost::filesystem::system_complete(bag_.getFileName().c_str()));
p = p.parent_path();
boost::filesystem::space_info info;
try
{
info = boost::filesystem::space(p);
}
catch (boost::filesystem::filesystem_error &e)
{
ROS_WARN("Failed to check filesystem stats [%s].", e.what());
writing_enabled_ = false;
return false;
}
if ( info.available < options_.min_space)
{
ROS_ERROR("Less than %s of space free on disk with %s. Disabling recording.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
writing_enabled_ = false;
return false;
}
else if (info.available < 5 * options_.min_space)
{
ROS_WARN("Less than 5 x %s of space free on disk with %s.", options_.min_space_str.c_str(), bag_.getFileName().c_str());
writing_enabled_ = true;
}
else
{
writing_enabled_ = true;
}
#endif
return true;
}
bool Recorder::checkLogging() {
if (writing_enabled_)
return true;
ros::WallTime now = ros::WallTime::now();
if (now >= warn_next_) {
warn_next_ = now + ros::WallDuration().fromSec(5.0);
ROS_WARN("Not logging message because logging disabled. Most likely cause is a full disk.");
}
return false;
}
} // namespace rosbag

37
thirdparty/ros/ros_comm/tools/rosbag/src/rosbag/__init__.py vendored Normal file
View File

@@ -0,0 +1,37 @@
# 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.
from .bag import Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException
# Import rosbag main to be used by the rosbag executable
from .rosbag_main import rosbagmain

2612
thirdparty/ros/ros_comm/tools/rosbag/src/rosbag/bag.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1328
thirdparty/ros/ros_comm/tools/rosbag/src/rosbag/migration.py vendored Normal file
View File

File diff suppressed because it is too large Load Diff

900
thirdparty/ros/ros_comm/tools/rosbag/src/rosbag/rosbag_main.py vendored Normal file
View File

@@ -0,0 +1,900 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2010, 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.
from __future__ import print_function
import optparse
import os
import shutil
import signal
import subprocess
import sys
import time
try:
from UserDict import UserDict # Python 2.x
except ImportError:
from collections import UserDict # Python 3.x
import roslib.message
import roslib.packages
from .bag import Bag, Compression, ROSBagException, ROSBagFormatException, ROSBagUnindexedException
from .migration import MessageMigrator, fixbag2, checkbag
def print_trans(old, new, indent):
from_txt = '%s [%s]' % (old._type, old._md5sum)
if new is not None:
to_txt= '%s [%s]' % (new._type, new._md5sum)
else:
to_txt = 'Unknown'
print(' ' * indent + ' * From: %s' % from_txt)
print(' ' * indent + ' To: %s' % to_txt)
def handle_split(option, opt_str, value, parser):
parser.values.split = True
if len(parser.rargs) > 0 and parser.rargs[0].isdigit():
print("Use of \"--split <MAX_SIZE>\" has been deprecated. Please use --split --size <MAX_SIZE> or --split --duration <MAX_DURATION>", file=sys.stderr)
parser.values.size = int(parser.rargs.pop(0))
def _stop_process(signum, frame, old_handler, process):
process.terminate()
if old_handler:
old_handler(signum, frame)
def record_cmd(argv):
parser = optparse.OptionParser(usage="rosbag record TOPIC1 [TOPIC2 TOPIC3 ...]",
description="Record a bag file with the contents of specified topics.",
formatter=optparse.IndentedHelpFormatter())
parser.add_option("-a", "--all", dest="all", default=False, action="store_true", help="record all topics")
parser.add_option("-e", "--regex", dest="regex", default=False, action="store_true", help="match topics using regular expressions")
parser.add_option("-x", "--exclude", dest="exclude_regex", default="", action="store", help="exclude topics matching the follow regular expression (subtracts from -a or regex)")
parser.add_option("-q", "--quiet", dest="quiet", default=False, action="store_true", help="suppress console output")
parser.add_option("-o", "--output-prefix", dest="prefix", default=None, action="store", help="prepend PREFIX to beginning of bag name (name will always end with date stamp)")
parser.add_option("-O", "--output-name", dest="name", default=None, action="store", help="record to bag with name NAME.bag")
parser.add_option( "--split", dest="split", default=False, callback=handle_split, action="callback", help="split the bag when maximum size or duration is reached")
parser.add_option( "--max-splits", dest="max_splits", type='int', action="store", help="Keep a maximum of N bag files, when reaching the maximum erase the oldest one to keep a constant number of files.", metavar="MAX_SPLITS")
parser.add_option( "--size", dest="size", type='int', action="store", help="record a bag of maximum size SIZE MB. (Default: infinite)", metavar="SIZE")
parser.add_option( "--duration", dest="duration", type='string',action="store", help="record a bag of maximum duration DURATION in seconds, unless 'm', or 'h' is appended.", metavar="DURATION")
parser.add_option("-b", "--buffsize", dest="buffsize", default=256, type='int', action="store", help="use an internal buffer of SIZE MB (Default: %default, 0 = infinite)", metavar="SIZE")
parser.add_option("--chunksize", dest="chunksize", default=768, type='int', action="store", help="Advanced. Record to chunks of SIZE KB (Default: %default)", metavar="SIZE")
parser.add_option("-l", "--limit", dest="num", default=0, type='int', action="store", help="only record NUM messages on each topic")
parser.add_option( "--node", dest="node", default=None, type='string',action="store", help="record all topics subscribed to by a specific node")
parser.add_option("-j", "--bz2", dest="compression", default=None, action="store_const", const='bz2', help="use BZ2 compression")
parser.add_option("--lz4", dest="compression", action="store_const", const='lz4', help="use LZ4 compression")
parser.add_option("--tcpnodelay", dest="tcpnodelay", action="store_true", help="Use the TCP_NODELAY transport hint when subscribing to topics.")
parser.add_option("--udp", dest="udp", action="store_true", help="Use the UDP transport hint when subscribing to topics.")
(options, args) = parser.parse_args(argv)
if len(args) == 0 and not options.all and not options.node:
parser.error("You must specify a topic name or else use the '-a' option.")
if options.prefix is not None and options.name is not None:
parser.error("Can't set both prefix and name.")
recordpath = roslib.packages.find_node('rosbag', 'record')
if not recordpath:
parser.error("Cannot find rosbag/record executable")
cmd = [recordpath[0]]
cmd.extend(['--buffsize', str(options.buffsize)])
cmd.extend(['--chunksize', str(options.chunksize)])
if options.num != 0: cmd.extend(['--limit', str(options.num)])
if options.quiet: cmd.extend(["--quiet"])
if options.prefix: cmd.extend(["-o", options.prefix])
if options.name: cmd.extend(["-O", options.name])
if options.exclude_regex: cmd.extend(["--exclude", options.exclude_regex])
if options.all: cmd.extend(["--all"])
if options.regex: cmd.extend(["--regex"])
if options.compression: cmd.extend(["--%s" % options.compression])
if options.split:
if not options.duration and not options.size:
parser.error("Split specified without giving a maximum duration or size")
cmd.extend(["--split"])
if options.max_splits:
cmd.extend(["--max-splits", str(options.max_splits)])
if options.duration: cmd.extend(["--duration", options.duration])
if options.size: cmd.extend(["--size", str(options.size)])
if options.node:
cmd.extend(["--node", options.node])
if options.tcpnodelay: cmd.extend(["--tcpnodelay"])
if options.udp: cmd.extend(["--udp"])
cmd.extend(args)
old_handler = signal.signal(
signal.SIGTERM,
lambda signum, frame: _stop_process(signum, frame, old_handler, process)
)
# Better way of handling it than os.execv
# This makes sure stdin handles are passed to the process.
process = subprocess.Popen(cmd)
process.wait()
def info_cmd(argv):
parser = optparse.OptionParser(usage='rosbag info [options] BAGFILE1 [BAGFILE2 BAGFILE3 ...]',
description='Summarize the contents of one or more bag files.')
parser.add_option('-y', '--yaml', dest='yaml', default=False, action='store_true', help='print information in YAML format')
parser.add_option('-k', '--key', dest='key', default=None, action='store', help='print information on the given key')
parser.add_option( '--freq', dest='freq', default=False, action='store_true', help='display topic message frequency statistics')
(options, args) = parser.parse_args(argv)
if len(args) == 0:
parser.error('You must specify at least 1 bag file.')
if options.key and not options.yaml:
parser.error('You can only specify key when printing in YAML format.')
for i, arg in enumerate(args):
try:
b = Bag(arg, 'r', skip_index=not options.freq)
if options.yaml:
info = b._get_yaml_info(key=options.key)
if info is not None:
print(info)
else:
print(b)
b.close()
if i < len(args) - 1:
print('---')
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % arg,
file=sys.stderr)
sys.exit(1)
except ROSBagException as ex:
print('ERROR reading %s: %s' % (arg, str(ex)), file=sys.stderr)
sys.exit(1)
except IOError as ex:
print('ERROR reading %s: %s' % (arg, str(ex)), file=sys.stderr)
sys.exit(1)
def handle_topics(option, opt_str, value, parser):
topics = []
for arg in parser.rargs:
if arg[:2] == "--" and len(arg) > 2:
break
if arg[:1] == "-" and len(arg) > 1:
break
topics.append(arg)
parser.values.topics.extend(topics)
del parser.rargs[:len(topics)]
def handle_pause_topics(option, opt_str, value, parser):
pause_topics = []
for arg in parser.rargs:
if arg[:2] == "--" and len(arg) > 2:
break
if arg[:1] == "-" and len(arg) > 1:
break
pause_topics.append(arg)
parser.values.pause_topics.extend(pause_topics)
del parser.rargs[:len(pause_topics)]
def play_cmd(argv):
parser = optparse.OptionParser(usage="rosbag play BAGFILE1 [BAGFILE2 BAGFILE3 ...]",
description="Play back the contents of one or more bag files in a time-synchronized fashion.")
parser.add_option("-p", "--prefix", dest="prefix", default='', type='str', help="prefix all output topics")
parser.add_option("-q", "--quiet", dest="quiet", default=False, action="store_true", help="suppress console output")
parser.add_option("-i", "--immediate", dest="immediate", default=False, action="store_true", help="play back all messages without waiting")
parser.add_option("--pause", dest="pause", default=False, action="store_true", help="start in paused mode")
parser.add_option("--queue", dest="queue", default=100, type='int', action="store", help="use an outgoing queue of size SIZE (defaults to %default)", metavar="SIZE")
parser.add_option("--clock", dest="clock", default=False, action="store_true", help="publish the clock time")
parser.add_option("--hz", dest="freq", default=100, type='float', action="store", help="use a frequency of HZ when publishing clock time (default: %default)", metavar="HZ")
parser.add_option("-d", "--delay", dest="delay", default=0.2, type='float', action="store", help="sleep SEC seconds after every advertise call (to allow subscribers to connect)", metavar="SEC")
parser.add_option("-r", "--rate", dest="rate", default=1.0, type='float', action="store", help="multiply the publish rate by FACTOR", metavar="FACTOR")
parser.add_option("-s", "--start", dest="start", default=0.0, type='float', action="store", help="start SEC seconds into the bag files", metavar="SEC")
parser.add_option("-u", "--duration", dest="duration", default=None, type='float', action="store", help="play only SEC seconds from the bag files", metavar="SEC")
parser.add_option("--skip-empty", dest="skip_empty", default=None, type='float', action="store", help="skip regions in the bag with no messages for more than SEC seconds", metavar="SEC")
parser.add_option("-l", "--loop", dest="loop", default=False, action="store_true", help="loop playback")
parser.add_option("-k", "--keep-alive", dest="keep_alive", default=False, action="store_true", help="keep alive past end of bag (useful for publishing latched topics)")
parser.add_option("--try-future-version", dest="try_future", default=False, action="store_true", help="still try to open a bag file, even if the version number is not known to the player")
parser.add_option("--topics", dest="topics", default=[], callback=handle_topics, action="callback", help="topics to play back")
parser.add_option("--pause-topics", dest="pause_topics", default=[], callback=handle_pause_topics, action="callback", help="topics to pause on during playback")
parser.add_option("--bags", help="bags files to play back from")
parser.add_option("--wait-for-subscribers", dest="wait_for_subscribers", default=False, action="store_true", help="wait for at least one subscriber on each topic before publishing")
(options, args) = parser.parse_args(argv)
if options.bags:
args.append(options.bags)
if len(args) == 0:
parser.error('You must specify at least 1 bag file to play back.')
playpath = roslib.packages.find_node('rosbag', 'play')
if not playpath:
parser.error("Cannot find rosbag/play executable")
cmd = [playpath[0]]
if options.prefix:
cmd.extend(["--prefix", str(options.prefix)])
if options.quiet: cmd.extend(["--quiet"])
if options.pause: cmd.extend(["--pause"])
if options.immediate: cmd.extend(["--immediate"])
if options.loop: cmd.extend(["--loop"])
if options.keep_alive: cmd.extend(["--keep-alive"])
if options.try_future: cmd.extend(["--try-future-version"])
if options.wait_for_subscribers: cmd.extend(["--wait-for-subscribers"])
if options.clock:
cmd.extend(["--clock", "--hz", str(options.freq)])
cmd.extend(['--queue', str(options.queue)])
cmd.extend(['--rate', str(options.rate)])
cmd.extend(['--delay', str(options.delay)])
cmd.extend(['--start', str(options.start)])
if options.duration:
cmd.extend(['--duration', str(options.duration)])
if options.skip_empty:
cmd.extend(['--skip-empty', str(options.skip_empty)])
if options.topics:
cmd.extend(['--topics'] + options.topics)
if options.pause_topics:
cmd.extend(['--pause-topics'] + options.pause_topics)
# prevent bag files to be passed as --topics or --pause-topics
if options.topics or options.pause_topics:
cmd.extend(['--bags'])
cmd.extend(args)
old_handler = signal.signal(
signal.SIGTERM,
lambda signum, frame: _stop_process(signum, frame, old_handler, process)
)
# Better way of handling it than os.execv
# This makes sure stdin handles are passed to the process.
process = subprocess.Popen(cmd)
process.wait()
def filter_cmd(argv):
def expr_eval(expr):
def eval_fn(topic, m, t):
return eval(expr)
return eval_fn
parser = optparse.OptionParser(usage="""rosbag filter [options] INBAG OUTBAG EXPRESSION
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)""",
description='Filter the contents of the bag.')
parser.add_option('-p', '--print', action='store', 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(argv)
if len(args) == 0:
parser.error('You must specify an in bag, an out bag, and an expression.')
if len(args) == 1:
parser.error('You must specify an out bag and an expression.')
if len(args) == 2:
parser.error("You must specify an expression.")
if len(args) > 3:
parser.error("Too many arguments.")
inbag_filename, outbag_filename, expr = args
if not os.path.isfile(inbag_filename):
print('Cannot locate input bag file [%s]' % inbag_filename, file=sys.stderr)
sys.exit(2)
if os.path.realpath(inbag_filename) == os.path.realpath(outbag_filename):
print('Cannot use same file as input and output [%s]' % inbag_filename, file=sys.stderr)
sys.exit(3)
filter_fn = expr_eval(expr)
outbag = Bag(outbag_filename, 'w')
try:
inbag = Bag(inbag_filename)
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename, file=sys.stderr)
sys.exit(1)
try:
meter = ProgressMeter(outbag_filename, inbag._uncompressed_size)
total_bytes = 0
if options.verbose_pattern:
verbose_pattern = expr_eval(options.verbose_pattern)
for topic, raw_msg, t in inbag.read_messages(raw=True):
msg_type, serialized_bytes, md5sum, pos, pytype = raw_msg
msg = pytype()
msg.deserialize(serialized_bytes)
if filter_fn(topic, msg, t):
print('MATCH', verbose_pattern(topic, msg, t))
outbag.write(topic, msg, t)
else:
print('NO MATCH', verbose_pattern(topic, msg, t))
total_bytes += len(serialized_bytes)
meter.step(total_bytes)
else:
for topic, raw_msg, t in inbag.read_messages(raw=True):
msg_type, serialized_bytes, md5sum, pos, pytype = raw_msg
msg = pytype()
msg.deserialize(serialized_bytes)
if filter_fn(topic, msg, t):
outbag.write(topic, msg, t)
total_bytes += len(serialized_bytes)
meter.step(total_bytes)
meter.finish()
finally:
inbag.close()
outbag.close()
def fix_cmd(argv):
parser = optparse.OptionParser(usage='rosbag fix INBAG OUTBAG [EXTRARULES1 EXTRARULES2 ...]', description='Repair the messages in a bag file so that it can be played in the current system.')
parser.add_option('-n', '--noplugins', action='store_true', dest='noplugins', help='do not load rulefiles via plugins')
parser.add_option('--force', action='store_true', dest='force', help='proceed with migrations, even if not all rules defined')
(options, args) = parser.parse_args(argv)
if len(args) < 1:
parser.error('You must pass input and output bag files.')
if len(args) < 2:
parser.error('You must pass an output bag file.')
inbag_filename = args[0]
outbag_filename = args[1]
rules = args[2:]
ext = os.path.splitext(outbag_filename)[1]
if ext == '.bmr':
parser.error('Input file should be a bag file, not a rule file.')
if ext != '.bag':
parser.error('Output file must be a bag file.')
outname = outbag_filename + '.tmp'
if os.path.exists(outbag_filename):
if not os.access(outbag_filename, os.W_OK):
print('Don\'t have permissions to access %s' % outbag_filename, file=sys.stderr)
sys.exit(1)
else:
try:
file = open(outbag_filename, 'w')
file.close()
except IOError as e:
print('Cannot open %s for writing' % outbag_filename, file=sys.stderr)
sys.exit(1)
if os.path.exists(outname):
if not os.access(outname, os.W_OK):
print('Don\'t have permissions to access %s' % outname, file=sys.stderr)
sys.exit(1)
else:
try:
file = open(outname, 'w')
file.close()
except IOError as e:
print('Cannot open %s for writing' % outname, file=sys.stderr)
sys.exit(1)
if options.noplugins is None:
options.noplugins = False
migrator = MessageMigrator(rules, plugins=not options.noplugins)
try:
migrations = fixbag2(migrator, inbag_filename, outname, options.force)
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename,
file=sys.stderr)
sys.exit(1)
if len(migrations) == 0:
os.rename(outname, outbag_filename)
print('Bag migrated successfully.')
else:
print('Bag could not be migrated. The following migrations could not be performed:')
for m in migrations:
print_trans(m[0][0].old_class, m[0][-1].new_class, 0)
if len(m[1]) > 0:
print(' %d rules missing:' % len(m[1]))
for r in m[1]:
print_trans(r.old_class, r.new_class,1)
print('Try running \'rosbag check\' to create the necessary rule files or run \'rosbag fix\' with the \'--force\' option.')
os.remove(outname)
sys.exit(1)
def check_cmd(argv):
parser = optparse.OptionParser(usage='rosbag check BAG [-g RULEFILE] [EXTRARULES1 EXTRARULES2 ...]', description='Determine whether a bag is playable in the current system, or if it can be migrated.')
parser.add_option('-g', '--genrules', action='store', dest='rulefile', default=None, help='generate a rulefile named RULEFILE')
parser.add_option('-a', '--append', action='store_true', dest='append', help='append to the end of an existing rulefile after loading it')
parser.add_option('-n', '--noplugins', action='store_true', dest='noplugins', help='do not load rulefiles via plugins')
(options, args) = parser.parse_args(argv)
if len(args) == 0:
parser.error('You must specify a bag file to check.')
if options.append and options.rulefile is None:
parser.error('Cannot specify -a without also specifying -g.')
if options.rulefile is not None:
rulefile_exists = os.path.isfile(options.rulefile)
if rulefile_exists and not options.append:
parser.error('The file %s already exists. Include -a if you intend to append.' % options.rulefile)
if not rulefile_exists and options.append:
parser.error('The file %s does not exist, and so -a is invalid.' % options.rulefile)
if options.append:
append_rule = [options.rulefile]
else:
append_rule = []
# First check that the bag is not unindexed
try:
Bag(args[0])
except ROSBagUnindexedException as ex:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % args[0], file=sys.stderr)
sys.exit(1)
mm = MessageMigrator(args[1:] + append_rule, not options.noplugins)
migrations = checkbag(mm, args[0])
if len(migrations) == 0:
print('Bag file does not need any migrations.')
exit(0)
print('The following migrations need to occur:')
all_rules = []
for m in migrations:
all_rules.extend(m[1])
print_trans(m[0][0].old_class, m[0][-1].new_class, 0)
if len(m[1]) > 0:
print(" %d rules missing:" % len(m[1]))
for r in m[1]:
print_trans(r.old_class, r.new_class, 1)
if options.rulefile is None:
if all_rules == []:
print("\nAll rules defined. Bag is ready to be migrated")
else:
print("\nTo generate rules, please run with -g <rulefile>")
exit(0)
output = ''
rules_left = mm.filter_rules_unique(all_rules)
if rules_left == []:
print("\nNo additional rule files needed to be generated. %s not created."%(options.rulefile))
exit(0)
while len(rules_left) > 0:
extra_rules = []
for r in rules_left:
if r.new_class is None:
print('The message type %s appears to have moved. Please enter the type to migrate it to.' % r.old_class._type)
new_type = raw_input('>')
new_class = roslib.message.get_message_class(new_type)
while new_class is None:
print("\'%s\' could not be found in your system. Please make sure it is built." % new_type)
new_type = raw_input('>')
new_class = roslib.message.get_message_class(new_type)
new_rule = mm.make_update_rule(r.old_class, new_class)
R = new_rule(mm, 'GENERATED.' + new_rule.__name__)
R.find_sub_paths()
new_rules = [r for r in mm.expand_rules(R.sub_rules) if r.valid == False]
extra_rules.extend(new_rules)
print('Creating the migration rule for %s requires additional missing rules:' % new_type)
for nr in new_rules:
print_trans(nr.old_class, nr.new_class,1)
output += R.get_class_def()
else:
output += r.get_class_def()
rules_left = mm.filter_rules_unique(extra_rules)
f = open(options.rulefile, 'a')
f.write(output)
f.close()
print('\nThe necessary rule files have been written to: %s' % options.rulefile)
def compress_cmd(argv):
parser = optparse.OptionParser(usage='rosbag compress [options] BAGFILE1 [BAGFILE2 ...]',
description='Compress one or more bag files.')
parser.add_option( '--output-dir', action='store', dest='output_dir', help='write to directory DIR', metavar='DIR')
parser.add_option('-f', '--force', action='store_true', dest='force', help='force overwriting of backup file if it exists')
parser.add_option('-q', '--quiet', action='store_true', dest='quiet', help='suppress noncritical messages')
parser.add_option('-j', '--bz2', action='store_const', dest='compression', help='use BZ2 compression', const=Compression.BZ2, default=Compression.BZ2)
parser.add_option( '--lz4', action='store_const', dest='compression', help='use lz4 compression', const=Compression.LZ4)
(options, args) = parser.parse_args(argv)
if len(args) < 1:
parser.error('You must specify at least one bag file.')
op = lambda inbag, outbag, quiet: change_compression_op(inbag, outbag, options.compression, options.quiet)
bag_op(args, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
def decompress_cmd(argv):
parser = optparse.OptionParser(usage='rosbag decompress [options] BAGFILE1 [BAGFILE2 ...]',
description='Decompress one or more bag files.')
parser.add_option( '--output-dir', action='store', dest='output_dir', help='write to directory DIR', metavar='DIR')
parser.add_option('-f', '--force', action='store_true', dest='force', help='force overwriting of backup file if it exists')
parser.add_option('-q', '--quiet', action='store_true', dest='quiet', help='suppress noncritical messages')
(options, args) = parser.parse_args(argv)
if len(args) < 1:
parser.error('You must specify at least one bag file.')
op = lambda inbag, outbag, quiet: change_compression_op(inbag, outbag, Compression.NONE, options.quiet)
bag_op(args, False, lambda b: False, op, options.output_dir, options.force, options.quiet)
def reindex_cmd(argv):
parser = optparse.OptionParser(usage='rosbag reindex [options] BAGFILE1 [BAGFILE2 ...]',
description='Reindexes one or more bag files.')
parser.add_option( '--output-dir', action='store', dest='output_dir', help='write to directory DIR', metavar='DIR')
parser.add_option('-f', '--force', action='store_true', dest='force', help='force overwriting of backup file if it exists')
parser.add_option('-q', '--quiet', action='store_true', dest='quiet', help='suppress noncritical messages')
(options, args) = parser.parse_args(argv)
if len(args) < 1:
parser.error('You must specify at least one bag file.')
op = lambda inbag, outbag, quiet: reindex_op(inbag, outbag, options.quiet)
bag_op(args, True, lambda b: b.version > 102, op, options.output_dir, options.force, options.quiet)
def bag_op(inbag_filenames, allow_unindexed, copy_fn, op, output_dir=None, force=False, quiet=False):
for inbag_filename in inbag_filenames:
# Check we can read the file
try:
inbag = Bag(inbag_filename, 'r', allow_unindexed=allow_unindexed)
except ROSBagUnindexedException:
print('ERROR bag unindexed: %s. Run rosbag reindex.' % inbag_filename, file=sys.stderr)
continue
except (ROSBagException, IOError) as ex:
print('ERROR reading %s: %s' % (inbag_filename, str(ex)), file=sys.stderr)
continue
# Determine whether we should copy the bag
copy = copy_fn(inbag)
inbag.close()
# Determine filename for output bag
if output_dir is None:
outbag_filename = inbag_filename
else:
outbag_filename = os.path.join(output_dir, os.path.split(inbag_filename)[1])
backup_filename = None
if outbag_filename == inbag_filename:
# Rename the input bag to ###.orig.###, and open for reading
backup_filename = '%s.orig%s' % os.path.splitext(inbag_filename)
if not force and os.path.exists(backup_filename):
if not quiet:
print('Skipping %s. Backup path %s already exists.' % (inbag_filename, backup_filename), file=sys.stderr)
continue
try:
if copy:
shutil.copy(inbag_filename, backup_filename)
else:
os.rename(inbag_filename, backup_filename)
except OSError as ex:
print('ERROR %s %s to %s: %s' % ('copying' if copy else 'moving', inbag_filename, backup_filename, str(ex)), file=sys.stderr)
continue
source_filename = backup_filename
else:
if copy:
shutil.copy(inbag_filename, outbag_filename)
source_filename = outbag_filename
else:
source_filename = inbag_filename
try:
inbag = Bag(source_filename, 'r', allow_unindexed=allow_unindexed)
# Open the output bag file for writing
try:
if copy:
outbag = Bag(outbag_filename, 'a', allow_unindexed=allow_unindexed)
else:
outbag = Bag(outbag_filename, 'w')
except (ROSBagException, IOError) as ex:
print('ERROR writing to %s: %s' % (outbag_filename, str(ex)), file=sys.stderr)
inbag.close()
continue
# Perform the operation
try:
op(inbag, outbag, quiet=quiet)
except ROSBagException as ex:
print('\nERROR operating on %s: %s' % (source_filename, str(ex)), file=sys.stderr)
inbag.close()
outbag.close()
continue
outbag.close()
inbag.close()
except KeyboardInterrupt:
if backup_filename is not None:
try:
if copy:
os.remove(backup_filename)
else:
os.rename(backup_filename, inbag_filename)
except OSError as ex:
print('ERROR %s %s to %s: %s', ('removing' if copy else 'moving', backup_filename, inbag_filename, str(ex)), file=sys.stderr)
break
except (ROSBagException, IOError) as ex:
print('ERROR operating on %s: %s' % (inbag_filename, str(ex)), file=sys.stderr)
def change_compression_op(inbag, outbag, compression, quiet):
outbag.compression = compression
if quiet:
for topic, msg, t in inbag.read_messages(raw=True):
outbag.write(topic, msg, t, raw=True)
else:
meter = ProgressMeter(outbag.filename, inbag._uncompressed_size)
total_bytes = 0
for topic, msg, t in inbag.read_messages(raw=True):
msg_type, serialized_bytes, md5sum, pos, pytype = msg
outbag.write(topic, msg, t, raw=True)
total_bytes += len(serialized_bytes)
meter.step(total_bytes)
meter.finish()
def reindex_op(inbag, outbag, quiet):
if inbag.version == 102:
if quiet:
try:
for offset in inbag.reindex():
pass
except:
pass
for (topic, msg, t) in inbag.read_messages():
outbag.write(topic, msg, t)
else:
meter = ProgressMeter(outbag.filename, inbag.size)
try:
for offset in inbag.reindex():
meter.step(offset)
except:
pass
meter.finish()
meter = ProgressMeter(outbag.filename, inbag.size)
for (topic, msg, t) in inbag.read_messages():
outbag.write(topic, msg, t)
meter.step(inbag._file.tell())
meter.finish()
else:
if quiet:
try:
for offset in outbag.reindex():
pass
except:
pass
else:
meter = ProgressMeter(outbag.filename, outbag.size)
try:
for offset in outbag.reindex():
meter.step(offset)
except:
pass
meter.finish()
class RosbagCmds(UserDict):
def __init__(self):
UserDict.__init__(self)
self._description = {}
self['help'] = self.help_cmd
def add_cmd(self, name, function, description):
self[name] = function
self._description[name] = description
def get_valid_cmds(self):
str = "Available subcommands:\n"
for k in sorted(self.keys()):
str += " %s " % k
if k in self._description.keys():
str +="\t%s" % self._description[k]
str += "\n"
return str
def help_cmd(self,argv):
argv = [a for a in argv if a != '-h' and a != '--help']
if len(argv) == 0:
print('Usage: rosbag <subcommand> [options] [args]')
print()
print("A bag is a file format in ROS for storing ROS message data. The rosbag command can record, replay and manipulate bags.")
print()
print(self.get_valid_cmds())
print('For additional information, see http://wiki.ros.org/rosbag')
print()
return
cmd = argv[0]
if cmd in self:
self[cmd](['-h'])
else:
print("Unknown command: '%s'" % cmd, file=sys.stderr)
print(self.get_valid_cmds(), file=sys.stderr)
class ProgressMeter(object):
def __init__(self, path, bytes_total, refresh_rate=1.0):
self.path = path
self.bytes_total = bytes_total
self.refresh_rate = refresh_rate
self.elapsed = 0.0
self.update_elapsed = 0.0
self.bytes_read = 0.0
self.start_time = time.time()
self._update_progress()
def step(self, bytes_read, force_update=False):
self.bytes_read = bytes_read
self.elapsed = time.time() - self.start_time
if force_update or self.elapsed - self.update_elapsed > self.refresh_rate:
self._update_progress()
self.update_elapsed = self.elapsed
def _update_progress(self):
max_path_len = self.terminal_width() - 37
path = self.path
if len(path) > max_path_len:
path = '...' + self.path[-max_path_len + 3:]
bytes_read_str = self._human_readable_size(float(self.bytes_read))
bytes_total_str = self._human_readable_size(float(self.bytes_total))
if self.bytes_read < self.bytes_total:
complete_fraction = float(self.bytes_read) / self.bytes_total
pct_complete = int(100.0 * complete_fraction)
if complete_fraction > 0.0:
eta = self.elapsed * (1.0 / complete_fraction - 1.0)
eta_min, eta_sec = eta / 60, eta % 60
if eta_min > 99:
eta_str = '--:--'
else:
eta_str = '%02d:%02d' % (eta_min, eta_sec)
else:
eta_str = '--:--'
progress = '%-*s %3d%% %8s / %8s %s ETA' % (max_path_len, path, pct_complete, bytes_read_str, bytes_total_str, eta_str)
else:
progress = '%-*s 100%% %19s %02d:%02d ' % (max_path_len, path, bytes_total_str, self.elapsed / 60, self.elapsed % 60)
print('\r', progress, end='')
sys.stdout.flush()
def _human_readable_size(self, size):
multiple = 1024.0
for suffix in ['KB', 'MB', 'GB', 'TB', 'PB', 'EB', 'ZB', 'YB']:
size /= multiple
if size < multiple:
return '%.1f %s' % (size, suffix)
raise ValueError('number too large')
def finish(self):
self.step(self.bytes_total, force_update=True)
print()
@staticmethod
def terminal_width():
"""Estimate the width of the terminal"""
width = 0
try:
import struct, fcntl, termios
s = struct.pack('HHHH', 0, 0, 0, 0)
x = fcntl.ioctl(1, termios.TIOCGWINSZ, s)
width = struct.unpack('HHHH', x)[1]
except (IOError, ImportError):
pass
if width <= 0:
try:
width = int(os.environ['COLUMNS'])
except:
pass
if width <= 0:
width = 80
return width
def rosbagmain(argv=None):
cmds = RosbagCmds()
cmds.add_cmd('record', record_cmd, "Record a bag file with the contents of specified topics.")
cmds.add_cmd('info', info_cmd, 'Summarize the contents of one or more bag files.')
cmds.add_cmd('play', play_cmd, "Play back the contents of one or more bag files in a time-synchronized fashion.")
cmds.add_cmd('check', check_cmd, 'Determine whether a bag is playable in the current system, or if it can be migrated.')
cmds.add_cmd('fix', fix_cmd, 'Repair the messages in a bag file so that it can be played in the current system.')
cmds.add_cmd('filter', filter_cmd, 'Filter the contents of the bag.')
cmds.add_cmd('compress', compress_cmd, 'Compress one or more bag files.')
cmds.add_cmd('decompress', decompress_cmd, 'Decompress one or more bag files.')
cmds.add_cmd('reindex', reindex_cmd, 'Reindexes one or more bag files.')
if argv is None:
argv = sys.argv
if '-h' in argv or '--help' in argv:
argv = [a for a in argv if a != '-h' and a != '--help']
argv.insert(1, 'help')
if len(argv) > 1:
cmd = argv[1]
else:
cmd = 'help'
try:
if cmd in cmds:
cmds[cmd](argv[2:])
else:
cmds['help']([cmd])
except KeyboardInterrupt:
pass

64
thirdparty/ros/ros_comm/tools/rosbag/src/time_translator.cpp vendored Normal file
View File

@@ -0,0 +1,64 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010, 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 "rosbag/time_translator.h"
namespace rosbag {
TimeTranslator::TimeTranslator()
: time_scale_(1.0), real_start_(ros::TIME_MIN), translated_start_(ros::TIME_MIN)
{
}
void TimeTranslator::setTimeScale(double const& s) {
time_scale_ = s;
}
void TimeTranslator::setRealStartTime(ros::Time const& t) {
real_start_ = t;
}
void TimeTranslator::setTranslatedStartTime(ros::Time const& t) {
translated_start_ = t;
}
void TimeTranslator::shift(ros::Duration const& d) {
translated_start_ += d;
}
ros::Time TimeTranslator::translate(ros::Time const& t) {
return translated_start_ + (t - real_start_) * (1.0 / time_scale_);
}
} // namespace rosbag

75
thirdparty/ros/ros_comm/tools/rosbag/test/test_roundtrip.py vendored Normal file
View File

@@ -0,0 +1,75 @@
import unittest
import tempfile
import os
import rosbag
import rospy
BAG_DIR = tempfile.mkdtemp(prefix='rosbag_tests')
class TestRoundTrip(unittest.TestCase):
def _write_simple_bag(self, name):
from std_msgs.msg import Int32, String
with rosbag.Bag(name, 'w') as bag:
s = String(data='foo')
i = Int32(data=42)
bag.write('chatter', s)
bag.write('numbers', i)
def _fname(self, name):
return os.path.join(BAG_DIR, name)
def test_value_equality(self):
fname = self._fname('test_value_equality.bag')
self._write_simple_bag(fname)
with rosbag.Bag(fname) as bag:
numbers = list(bag.read_messages('numbers'))
chatter = list(bag.read_messages('chatter'))
self.assertEqual(len(numbers), 1)
self.assertEqual(len(chatter), 1)
numbers = numbers[0]
chatter = chatter[0]
# channel names
self.assertEqual(numbers[0], 'numbers')
self.assertEqual(chatter[0], 'chatter')
# values
self.assertEqual(numbers[1].data, 42)
self.assertEqual(chatter[1].data, 'foo')
@unittest.expectedFailure
def test_type_equality(self):
fname = self._fname('test_type_equality.bag')
from std_msgs.msg import Int32, String
self._write_simple_bag(fname)
with rosbag.Bag(fname) as bag:
numbers = next(bag.read_messages('numbers'))
chatter = next(bag.read_messages('chatter'))
self.assertEqual(numbers[1].__class__, Int32)
self.assertEqual(chatter[1].__class__, String)
@unittest.expectedFailure
def test_type_isinstance(self):
fname = self._fname('test_type_isinstance.bag')
from std_msgs.msg import Int32, String
self._write_simple_bag(fname)
with rosbag.Bag(fname) as bag:
numbers = next(bag.read_messages('numbers'))
chatter = next(bag.read_messages('chatter'))
self.assertIsInstance(numbers[1], Int32)
self.assertIsInstance(chatter[1], String)