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)

145
thirdparty/ros/ros_comm/tools/rosbag_storage/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,145 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rosbag_storage
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.12.14 (2018-08-23)
--------------------
1.12.13 (2018-02-21)
--------------------
* performance improvement for lower/upper bound (`#1223 <https://github.com/ros/ros_comm/issues/1223>`_)
* place console_bridge macros definition in header and use it everywhere console_bridge is included (`#1238 <https://github.com/ros/ros_comm/issues/1238>`_)
1.12.12 (2017-11-16)
--------------------
* backward compatibility with libconsole-bridge in Jessie (take 3) (`#1235 <https://github.com/ros/ros_comm/issues/1235>`_)
1.12.11 (2017-11-07)
--------------------
1.12.10 (2017-11-06)
--------------------
* fix compatibility with libconsole-bridge in Jessie, take 2
1.12.9 (2017-11-06)
-------------------
* fix compatibility with libconsole-bridge in Jessie (`#1219 <https://github.com/ros/ros_comm/issues/1219>`_, regression from 1.12.8)
1.12.8 (2017-11-06)
-------------------
* check if bzfile\_ and lz4s\_ handle is valid before reading/writing/closing (`#1183 <https://github.com/ros/ros_comm/issues/1183>`_)
* fix an out of bounds read in rosbag::View::iterator::increment() (`#1191 <https://github.com/ros/ros_comm/issues/1191>`_)
* replace usage deprecated console_bridge macros (`#1149 <https://github.com/ros/ros_comm/issues/1149>`_)
* fix whitespace warnings with g++ 7 (`#1138 <https://github.com/ros/ros_comm/issues/1138>`_)
* remove deprecated dynamic exception specifications (`#1137 <https://github.com/ros/ros_comm/issues/1137>`_)
* fix buffer overflow vulnerability (`#1092 <https://github.com/ros/ros_comm/issues/1092>`_)
* fix rosbag::View::iterator copy assignment operator (`#1017 <https://github.com/ros/ros_comm/issues/1017>`_)
* fix open mode on Windows (`#1005 <https://github.com/ros/ros_comm/pull/1005>`_)
* add swap function instead of copy constructor / assignment operator for rosbag::Bag (`#1000 <https://github.com/ros/ros_comm/issues/1000>`_)
1.12.7 (2017-02-17)
-------------------
1.12.6 (2016-10-26)
-------------------
1.12.5 (2016-09-30)
-------------------
1.12.4 (2016-09-19)
-------------------
1.12.3 (2016-09-17)
-------------------
* make Bag constructor explicit (`#835 <https://github.com/ros/ros_comm/pull/835>`_)
1.12.2 (2016-06-03)
-------------------
1.12.1 (2016-04-18)
-------------------
* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
1.12.0 (2016-03-18)
-------------------
1.11.18 (2016-03-17)
--------------------
* fix compiler warnings
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)
--------------------
1.11.15 (2015-10-13)
--------------------
1.11.14 (2015-09-19)
--------------------
1.11.13 (2015-04-28)
--------------------
1.11.12 (2015-04-27)
--------------------
1.11.11 (2015-04-16)
--------------------
* support large bagfiles (>2GB) on 32-bit systems (`#464 <https://github.com/ros/ros_comm/issues/464>`_)
1.11.10 (2014-12-22)
--------------------
* fix various defects reported by coverity
1.11.9 (2014-08-18)
-------------------
1.11.8 (2014-08-04)
-------------------
1.11.7 (2014-07-18)
-------------------
1.11.6 (2014-07-10)
-------------------
1.11.5 (2014-06-24)
-------------------
* convert to use console bridge from upstream debian package (`ros/rosdistro#4633 <https://github.com/ros/rosdistro/issues/4633>`_)
1.11.4 (2014-06-16)
-------------------
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>`_)
* move rosbag dox to rosbag_storage (`#389 <https://github.com/ros/ros_comm/issues/389>`_)
1.11.0 (2014-03-04)
-------------------
1.10.0 (2014-02-11)
-------------------
* remove use of __connection header
1.9.54 (2014-01-27)
-------------------
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>`_)

52
thirdparty/ros/ros_comm/tools/rosbag_storage/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,52 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosbag_storage)
if(NOT WIN32)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
endif()
find_package(console_bridge REQUIRED)
find_package(catkin REQUIRED COMPONENTS cpp_common roscpp_serialization roscpp_traits rostime roslz4)
find_package(Boost REQUIRED COMPONENTS date_time filesystem program_options regex)
find_package(BZip2 REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES rosbag_storage
CATKIN_DEPENDS roslz4
DEPENDS console_bridge Boost
)
# Support large bags (>2GB) on 32-bit systems
add_definitions(-D_FILE_OFFSET_BITS=64)
include_directories(include ${catkin_INCLUDE_DIRS} ${console_bridge_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${BZIP2_INCLUDE_DIR})
add_definitions(${BZIP2_DEFINITIONS})
add_library(rosbag_storage
src/bag.cpp
src/bag_player.cpp
src/buffer.cpp
src/bz2_stream.cpp
src/lz4_stream.cpp
src/chunked_file.cpp
src/message_instance.cpp
src/query.cpp
src/stream.cpp
src/view.cpp
src/uncompressed_stream.cpp
)
target_link_libraries(rosbag_storage ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BZIP2_LIBRARIES} ${console_bridge_LIBRARIES})
install(TARGETS rosbag_storage
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)

638
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/bag.h vendored Normal file
View File

@@ -0,0 +1,638 @@
/*********************************************************************
* 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_BAG_H
#define ROSBAG_BAG_H
#include "rosbag/macros.h"
#include "rosbag/buffer.h"
#include "rosbag/chunked_file.h"
#include "rosbag/constants.h"
#include "rosbag/exceptions.h"
#include "rosbag/structures.h"
#include "ros/header.h"
#include "ros/time.h"
#include "ros/message_traits.h"
#include "ros/message_event.h"
#include "ros/serialization.h"
//#include "ros/subscription_callback_helper.h"
#include <ios>
#include <map>
#include <queue>
#include <set>
#include <stdexcept>
#include <boost/format.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include "console_bridge/console.h"
// Remove this include when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#include "rosbag/console_bridge_compatibility.h"
namespace rosbag {
namespace bagmode
{
//! The possible modes to open a bag in
enum BagMode
{
Write = 1,
Read = 2,
Append = 4
};
}
typedef bagmode::BagMode BagMode;
class MessageInstance;
class View;
class Query;
class ROSBAG_DECL Bag
{
friend class MessageInstance;
friend class View;
public:
Bag();
//! Open a bag file
/*!
* \param filename The bag file to open
* \param mode The mode to use (either read, write or append)
*
* Can throw BagException
*/
explicit Bag(std::string const& filename, uint32_t mode = bagmode::Read);
~Bag();
//! Open a bag file.
/*!
* \param filename The bag file to open
* \param mode The mode to use (either read, write or append)
*
* Can throw BagException
*/
void open(std::string const& filename, uint32_t mode = bagmode::Read);
//! Close the bag file
void close();
std::string getFileName() const; //!< Get the filename of the bag
BagMode getMode() const; //!< Get the mode the bag is in
uint32_t getMajorVersion() const; //!< Get the major-version of the open bag file
uint32_t getMinorVersion() const; //!< Get the minor-version of the open bag file
uint64_t getSize() const; //!< Get the current size of the bag file (a lower bound)
void setCompression(CompressionType compression); //!< Set the compression method to use for writing chunks
CompressionType getCompression() const; //!< Get the compression method to use for writing chunks
void setChunkThreshold(uint32_t chunk_threshold); //!< Set the threshold for creating new chunks
uint32_t getChunkThreshold() const; //!< Get the threshold for creating new chunks
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param event The message event to be added
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::MessageEvent<T> const& event);
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param time Timestamp of the message
* \param msg The message to be added
* \param connection_header A connection header.
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::Time const& time, T const& msg,
boost::shared_ptr<ros::M_string> connection_header = boost::shared_ptr<ros::M_string>());
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param time Timestamp of the message
* \param msg The message to be added
* \param connection_header A connection header.
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T const> const& msg,
boost::shared_ptr<ros::M_string> connection_header = boost::shared_ptr<ros::M_string>());
//! Write a message into the bag file
/*!
* \param topic The topic name
* \param time Timestamp of the message
* \param msg The message to be added
* \param connection_header A connection header.
*
* Can throw BagIOException
*/
template<class T>
void write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T> const& msg,
boost::shared_ptr<ros::M_string> connection_header = boost::shared_ptr<ros::M_string>());
void swap(Bag&);
private:
Bag(const Bag&);
Bag& operator=(const Bag&);
// This helper function actually does the write with an arbitrary serializable message
template<class T>
void doWrite(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> const& connection_header);
void openRead (std::string const& filename);
void openWrite (std::string const& filename);
void openAppend(std::string const& filename);
void closeWrite();
template<class T>
boost::shared_ptr<T> instantiateBuffer(IndexEntry const& index_entry) const; //!< deserializes the message held in record_buffer_
void startWriting();
void stopWriting();
void startReadingVersion102();
void startReadingVersion200();
// Writing
void writeVersion();
void writeFileHeaderRecord();
void writeConnectionRecord(ConnectionInfo const* connection_info);
void appendConnectionRecordToBuffer(Buffer& buf, ConnectionInfo const* connection_info);
template<class T>
void writeMessageDataRecord(uint32_t conn_id, ros::Time const& time, T const& msg);
void writeIndexRecords();
void writeConnectionRecords();
void writeChunkInfoRecords();
void startWritingChunk(ros::Time time);
void writeChunkHeader(CompressionType compression, uint32_t compressed_size, uint32_t uncompressed_size);
void stopWritingChunk();
// Reading
void readVersion();
void readFileHeaderRecord();
void readConnectionRecord();
void readChunkHeader(ChunkHeader& chunk_header) const;
void readChunkInfoRecord();
void readConnectionIndexRecord200();
void readTopicIndexRecord102();
void readMessageDefinitionRecord102();
void readMessageDataRecord102(uint64_t offset, ros::Header& header) const;
ros::Header readMessageDataHeader(IndexEntry const& index_entry);
uint32_t readMessageDataSize(IndexEntry const& index_entry) const;
template<typename Stream>
void readMessageDataIntoStream(IndexEntry const& index_entry, Stream& stream) const;
void decompressChunk(uint64_t chunk_pos) const;
void decompressRawChunk(ChunkHeader const& chunk_header) const;
void decompressBz2Chunk(ChunkHeader const& chunk_header) const;
void decompressLz4Chunk(ChunkHeader const& chunk_header) const;
uint32_t getChunkOffset() const;
// Record header I/O
void writeHeader(ros::M_string const& fields);
void writeDataLength(uint32_t data_len);
void appendHeaderToBuffer(Buffer& buf, ros::M_string const& fields);
void appendDataLengthToBuffer(Buffer& buf, uint32_t data_len);
void readHeaderFromBuffer(Buffer& buffer, uint32_t offset, ros::Header& header, uint32_t& data_size, uint32_t& bytes_read) const;
void readMessageDataHeaderFromBuffer(Buffer& buffer, uint32_t offset, ros::Header& header, uint32_t& data_size, uint32_t& bytes_read) const;
bool readHeader(ros::Header& header) const;
bool readDataLength(uint32_t& data_size) const;
bool isOp(ros::M_string& fields, uint8_t reqOp) const;
// Header fields
template<typename T>
std::string toHeaderString(T const* field) const;
std::string toHeaderString(ros::Time const* field) const;
template<typename T>
bool readField(ros::M_string const& fields, std::string const& field_name, bool required, T* data) const;
bool readField(ros::M_string const& fields, std::string const& field_name, unsigned int min_len, unsigned int max_len, bool required, std::string& data) const;
bool readField(ros::M_string const& fields, std::string const& field_name, bool required, std::string& data) const;
bool readField(ros::M_string const& fields, std::string const& field_name, bool required, ros::Time& data) const;
ros::M_string::const_iterator checkField(ros::M_string const& fields, std::string const& field,
unsigned int min_len, unsigned int max_len, bool required) const;
// Low-level I/O
void write(char const* s, std::streamsize n);
void write(std::string const& s);
void read(char* b, std::streamsize n) const;
void seek(uint64_t pos, int origin = std::ios_base::beg) const;
private:
BagMode mode_;
mutable ChunkedFile file_;
int version_;
CompressionType compression_;
uint32_t chunk_threshold_;
uint32_t bag_revision_;
uint64_t file_size_;
uint64_t file_header_pos_;
uint64_t index_data_pos_;
uint32_t connection_count_;
uint32_t chunk_count_;
// Current chunk
bool chunk_open_;
ChunkInfo curr_chunk_info_;
uint64_t curr_chunk_data_pos_;
std::map<std::string, uint32_t> topic_connection_ids_;
std::map<ros::M_string, uint32_t> header_connection_ids_;
std::map<uint32_t, ConnectionInfo*> connections_;
std::vector<ChunkInfo> chunks_;
std::map<uint32_t, std::multiset<IndexEntry> > connection_indexes_;
std::map<uint32_t, std::multiset<IndexEntry> > curr_chunk_connection_indexes_;
mutable Buffer header_buffer_; //!< reusable buffer in which to assemble the record header before writing to file
mutable Buffer record_buffer_; //!< reusable buffer in which to assemble the record data before writing to file
mutable Buffer chunk_buffer_; //!< reusable buffer to read chunk into
mutable Buffer decompress_buffer_; //!< reusable buffer to decompress chunks into
mutable Buffer outgoing_chunk_buffer_; //!< reusable buffer to read chunk into
mutable Buffer* current_buffer_;
mutable uint64_t decompressed_chunk_; //!< position of decompressed chunk
};
} // namespace rosbag
#include "rosbag/message_instance.h"
namespace rosbag {
// Templated method definitions
template<class T>
void Bag::write(std::string const& topic, ros::MessageEvent<T> const& event) {
doWrite(topic, event.getReceiptTime(), *event.getMessage(), event.getConnectionHeaderPtr());
}
template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> connection_header) {
doWrite(topic, time, msg, connection_header);
}
template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T const> const& msg, boost::shared_ptr<ros::M_string> connection_header) {
doWrite(topic, time, *msg, connection_header);
}
template<class T>
void Bag::write(std::string const& topic, ros::Time const& time, boost::shared_ptr<T> const& msg, boost::shared_ptr<ros::M_string> connection_header) {
doWrite(topic, time, *msg, connection_header);
}
template<typename T>
std::string Bag::toHeaderString(T const* field) const {
return std::string((char*) field, sizeof(T));
}
template<typename T>
bool Bag::readField(ros::M_string const& fields, std::string const& field_name, bool required, T* data) const {
ros::M_string::const_iterator i = checkField(fields, field_name, sizeof(T), sizeof(T), required);
if (i == fields.end())
return false;
memcpy(data, i->second.data(), sizeof(T));
return true;
}
template<typename Stream>
void Bag::readMessageDataIntoStream(IndexEntry const& index_entry, Stream& stream) const {
ros::Header header;
uint32_t data_size;
uint32_t bytes_read;
switch (version_)
{
case 200:
{
decompressChunk(index_entry.chunk_pos);
readMessageDataHeaderFromBuffer(*current_buffer_, index_entry.offset, header, data_size, bytes_read);
if (data_size > 0)
memcpy(stream.advance(data_size), current_buffer_->getData() + index_entry.offset + bytes_read, data_size);
break;
}
case 102:
{
readMessageDataRecord102(index_entry.chunk_pos, header);
data_size = record_buffer_.getSize();
if (data_size > 0)
memcpy(stream.advance(data_size), record_buffer_.getData(), data_size);
break;
}
default:
throw BagFormatException((boost::format("Unhandled version: %1%") % version_).str());
}
}
template<class T>
boost::shared_ptr<T> Bag::instantiateBuffer(IndexEntry const& index_entry) const {
switch (version_)
{
case 200:
{
decompressChunk(index_entry.chunk_pos);
// Read the message header
ros::Header header;
uint32_t data_size;
uint32_t bytes_read;
readMessageDataHeaderFromBuffer(*current_buffer_, index_entry.offset, header, data_size, bytes_read);
// Read the connection id from the header
uint32_t connection_id;
readField(*header.getValues(), CONNECTION_FIELD_NAME, true, &connection_id);
std::map<uint32_t, ConnectionInfo*>::const_iterator connection_iter = connections_.find(connection_id);
if (connection_iter == connections_.end())
throw BagFormatException((boost::format("Unknown connection ID: %1%") % connection_id).str());
ConnectionInfo* connection_info = connection_iter->second;
boost::shared_ptr<T> p = boost::make_shared<T>();
ros::serialization::PreDeserializeParams<T> predes_params;
predes_params.message = p;
predes_params.connection_header = connection_info->header;
ros::serialization::PreDeserialize<T>::notify(predes_params);
// Deserialize the message
ros::serialization::IStream s(current_buffer_->getData() + index_entry.offset + bytes_read, data_size);
ros::serialization::deserialize(s, *p);
return p;
}
case 102:
{
// Read the message record
ros::Header header;
readMessageDataRecord102(index_entry.chunk_pos, header);
ros::M_string& fields = *header.getValues();
// Read the connection id from the header
std::string topic, latching("0"), callerid;
readField(fields, TOPIC_FIELD_NAME, true, topic);
readField(fields, LATCHING_FIELD_NAME, false, latching);
readField(fields, CALLERID_FIELD_NAME, false, callerid);
std::map<std::string, uint32_t>::const_iterator topic_conn_id_iter = topic_connection_ids_.find(topic);
if (topic_conn_id_iter == topic_connection_ids_.end())
throw BagFormatException((boost::format("Unknown topic: %1%") % topic).str());
uint32_t connection_id = topic_conn_id_iter->second;
std::map<uint32_t, ConnectionInfo*>::const_iterator connection_iter = connections_.find(connection_id);
if (connection_iter == connections_.end())
throw BagFormatException((boost::format("Unknown connection ID: %1%") % connection_id).str());
ConnectionInfo* connection_info = connection_iter->second;
boost::shared_ptr<T> p = boost::make_shared<T>();
// Create a new connection header, updated with the latching and callerid values
boost::shared_ptr<ros::M_string> message_header(boost::make_shared<ros::M_string>());
for (ros::M_string::const_iterator i = connection_info->header->begin(); i != connection_info->header->end(); i++)
(*message_header)[i->first] = i->second;
(*message_header)["latching"] = latching;
(*message_header)["callerid"] = callerid;
ros::serialization::PreDeserializeParams<T> predes_params;
predes_params.message = p;
predes_params.connection_header = message_header;
ros::serialization::PreDeserialize<T>::notify(predes_params);
// Deserialize the message
ros::serialization::IStream s(record_buffer_.getData(), record_buffer_.getSize());
ros::serialization::deserialize(s, *p);
return p;
}
default:
throw BagFormatException((boost::format("Unhandled version: %1%") % version_).str());
}
}
template<class T>
void Bag::doWrite(std::string const& topic, ros::Time const& time, T const& msg, boost::shared_ptr<ros::M_string> const& connection_header) {
if (time < ros::TIME_MIN)
{
throw BagException("Tried to insert a message with time less than ros::TIME_MIN");
}
// Whenever we write we increment our revision
bag_revision_++;
// Get ID for connection header
ConnectionInfo* connection_info = NULL;
uint32_t conn_id = 0;
if (!connection_header) {
// No connection header: we'll manufacture one, and store by topic
std::map<std::string, uint32_t>::iterator topic_connection_ids_iter = topic_connection_ids_.find(topic);
if (topic_connection_ids_iter == topic_connection_ids_.end()) {
conn_id = connections_.size();
topic_connection_ids_[topic] = conn_id;
}
else {
conn_id = topic_connection_ids_iter->second;
connection_info = connections_[conn_id];
}
}
else {
// Store the connection info by the address of the connection header
// Add the topic name to the connection header, so that when we later search by
// connection header, we can disambiguate connections that differ only by topic name (i.e.,
// same callerid, same message type), #3755. This modified connection header is only used
// for our bookkeeping, and will not appear in the resulting .bag.
ros::M_string connection_header_copy(*connection_header);
connection_header_copy["topic"] = topic;
std::map<ros::M_string, uint32_t>::iterator header_connection_ids_iter = header_connection_ids_.find(connection_header_copy);
if (header_connection_ids_iter == header_connection_ids_.end()) {
conn_id = connections_.size();
header_connection_ids_[connection_header_copy] = conn_id;
}
else {
conn_id = header_connection_ids_iter->second;
connection_info = connections_[conn_id];
}
}
{
// Seek to the end of the file (needed in case previous operation was a read)
seek(0, std::ios::end);
file_size_ = file_.getOffset();
// Write the chunk header if we're starting a new chunk
if (!chunk_open_)
startWritingChunk(time);
// Write connection info record, if necessary
if (connection_info == NULL) {
connection_info = new ConnectionInfo();
connection_info->id = conn_id;
connection_info->topic = topic;
connection_info->datatype = std::string(ros::message_traits::datatype(msg));
connection_info->md5sum = std::string(ros::message_traits::md5sum(msg));
connection_info->msg_def = std::string(ros::message_traits::definition(msg));
if (connection_header != NULL) {
connection_info->header = connection_header;
}
else {
connection_info->header = boost::make_shared<ros::M_string>();
(*connection_info->header)["type"] = connection_info->datatype;
(*connection_info->header)["md5sum"] = connection_info->md5sum;
(*connection_info->header)["message_definition"] = connection_info->msg_def;
}
connections_[conn_id] = connection_info;
writeConnectionRecord(connection_info);
appendConnectionRecordToBuffer(outgoing_chunk_buffer_, connection_info);
}
// Add to topic indexes
IndexEntry index_entry;
index_entry.time = time;
index_entry.chunk_pos = curr_chunk_info_.pos;
index_entry.offset = getChunkOffset();
std::multiset<IndexEntry>& chunk_connection_index = curr_chunk_connection_indexes_[connection_info->id];
chunk_connection_index.insert(chunk_connection_index.end(), index_entry);
std::multiset<IndexEntry>& connection_index = connection_indexes_[connection_info->id];
connection_index.insert(connection_index.end(), index_entry);
// Increment the connection count
curr_chunk_info_.connection_counts[connection_info->id]++;
// Write the message data
writeMessageDataRecord(conn_id, time, msg);
// Check if we want to stop this chunk
uint32_t chunk_size = getChunkOffset();
CONSOLE_BRIDGE_logDebug(" curr_chunk_size=%d (threshold=%d)", chunk_size, chunk_threshold_);
if (chunk_size > chunk_threshold_) {
// Empty the outgoing chunk
stopWritingChunk();
outgoing_chunk_buffer_.setSize(0);
// We no longer have a valid curr_chunk_info
curr_chunk_info_.pos = -1;
}
}
}
template<class T>
void Bag::writeMessageDataRecord(uint32_t conn_id, ros::Time const& time, T const& msg) {
ros::M_string header;
header[OP_FIELD_NAME] = toHeaderString(&OP_MSG_DATA);
header[CONNECTION_FIELD_NAME] = toHeaderString(&conn_id);
header[TIME_FIELD_NAME] = toHeaderString(&time);
// Assemble message in memory first, because we need to write its length
uint32_t msg_ser_len = ros::serialization::serializationLength(msg);
record_buffer_.setSize(msg_ser_len);
ros::serialization::OStream s(record_buffer_.getData(), msg_ser_len);
// todo: serialize into the outgoing_chunk_buffer & remove record_buffer_
ros::serialization::serialize(s, msg);
// We do an extra seek here since writing our data record may
// have indirectly moved our file-pointer if it was a
// MessageInstance for our own bag
seek(0, std::ios::end);
file_size_ = file_.getOffset();
CONSOLE_BRIDGE_logDebug("Writing MSG_DATA [%llu:%d]: conn=%d sec=%d nsec=%d data_len=%d",
(unsigned long long) file_.getOffset(), getChunkOffset(), conn_id, time.sec, time.nsec, msg_ser_len);
writeHeader(header);
writeDataLength(msg_ser_len);
write((char*) record_buffer_.getData(), msg_ser_len);
// todo: use better abstraction than appendHeaderToBuffer
appendHeaderToBuffer(outgoing_chunk_buffer_, header);
appendDataLengthToBuffer(outgoing_chunk_buffer_, msg_ser_len);
uint32_t offset = outgoing_chunk_buffer_.getSize();
outgoing_chunk_buffer_.setSize(outgoing_chunk_buffer_.getSize() + msg_ser_len);
memcpy(outgoing_chunk_buffer_.getData() + offset, record_buffer_.getData(), msg_ser_len);
// Update the current chunk time range
if (time > curr_chunk_info_.end_time)
curr_chunk_info_.end_time = time;
else if (time < curr_chunk_info_.start_time)
curr_chunk_info_.start_time = time;
}
inline void swap(Bag& a, Bag& b) {
a.swap(b);
}
} // namespace rosbag
#endif

135
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/bag_player.h vendored Normal file
View File

@@ -0,0 +1,135 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2013, Open Source Robotics Foundation
* 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_BAG_PLAYER_H
#define ROSBAG_BAG_PLAYER_H
#include <boost/foreach.hpp>
#include "rosbag/bag.h"
#include "rosbag/view.h"
namespace rosbag
{
// A helper struct
struct BagCallback
{
virtual ~BagCallback() {};
virtual void call(MessageInstance m) = 0;
};
// A helper class for the callbacks
template<class T>
class BagCallbackT : public BagCallback
{
public:
typedef boost::function<void (const boost::shared_ptr<const T>&)> Callback;
BagCallbackT(Callback cb) :
cb_(cb)
{}
void call(MessageInstance m) {
cb_(m.instantiate<T>());
}
private:
Callback cb_;
};
/* A class for playing back bag files at an API level. It supports
relatime, as well as accelerated and slowed playback. */
class BagPlayer
{
public:
/* Constructor expecting the filename of a bag */
BagPlayer(const std::string &filename);
/* Register a callback for a specific topic and type */
template<class T>
void register_callback(const std::string &topic,
typename BagCallbackT<T>::Callback f);
/* Unregister a callback for a topic already registered */
void unregister_callback(const std::string &topic);
/* Set the time in the bag to start.
* Default is the first message */
void set_start(const ros::Time &start);
/* Set the time in the bag to stop.
* Default is the last message */
void set_end(const ros::Time &end);
/* Set the speed to playback. 1.0 is the default.
* 2.0 would be twice as fast, 0.5 is half realtime. */
void set_playback_speed(double scale);
/* Start playback of the bag file using the parameters previously
set */
void start_play();
/* Get the current time of the playback */
ros::Time get_time();
// Destructor
virtual ~BagPlayer();
// The bag file interface loaded in the constructor.
Bag bag;
private:
ros::Time real_time(const ros::Time &msg_time);
std::map<std::string, BagCallback *> cbs_;
ros::Time bag_start_;
ros::Time bag_end_;
ros::Time last_message_time_;
double playback_speed_;
ros::Time play_start_;
};
template<class T>
void BagPlayer::register_callback(const std::string &topic,
typename BagCallbackT<T>::Callback cb) {
cbs_[topic] = new BagCallbackT<T>(cb);
}
}
#endif

73
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/buffer.h vendored Normal file
View File

@@ -0,0 +1,73 @@
/*********************************************************************
* 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_BUFFER_H
#define ROSBAG_BUFFER_H
#include <stdint.h>
#include "macros.h"
namespace rosbag {
class ROSBAG_DECL Buffer
{
public:
Buffer();
~Buffer();
uint8_t* getData();
uint32_t getCapacity() const;
uint32_t getSize() const;
void setSize(uint32_t size);
void swap(Buffer& other);
private:
Buffer(const Buffer&);
Buffer& operator=(const Buffer&);
void ensureCapacity(uint32_t capacity);
private:
uint8_t* buffer_;
uint32_t capacity_;
uint32_t size_;
};
inline void swap(Buffer& a, Buffer& b) {
a.swap(b);
}
} // namespace rosbag
#endif

111
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/chunked_file.h vendored Normal file
View File

@@ -0,0 +1,111 @@
/*********************************************************************
* 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_CHUNKED_FILE_H
#define ROSBAG_CHUNKED_FILE_H
#include <ios>
#include <stdint.h>
#include <string>
#include "macros.h"
#include <boost/shared_ptr.hpp>
#include <bzlib.h>
#include "rosbag/stream.h"
namespace rosbag {
//! ChunkedFile reads and writes files which contain interleaved chunks of compressed and uncompressed data.
class ROSBAG_DECL ChunkedFile
{
friend class Stream;
public:
ChunkedFile();
~ChunkedFile();
void openWrite (std::string const& filename); //!< open file for writing
void openRead (std::string const& filename); //!< open file for reading
void openReadWrite(std::string const& filename); //!< open file for reading & writing
void close(); //!< close the file
std::string getFileName() const; //!< return path of currently open file
uint64_t getOffset() const; //!< return current offset from the beginning of the file
uint32_t getCompressedBytesIn() const; //!< return the number of bytes written to current compressed stream
bool isOpen() const; //!< return true if file is open for reading or writing
bool good() const; //!< return true if hasn't reached end-of-file and no error
void setReadMode(CompressionType type);
void setWriteMode(CompressionType type);
// File I/O
void write(std::string const& s);
void write(void* ptr, size_t size); //!< write size bytes from ptr to the file
void read(void* ptr, size_t size); //!< read size bytes from the file into ptr
std::string getline();
bool truncate(uint64_t length);
void seek(uint64_t offset, int origin = std::ios_base::beg); //!< seek to given offset from origin
void decompress(CompressionType compression, uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
void swap(ChunkedFile& other);
private:
ChunkedFile(const ChunkedFile&);
ChunkedFile& operator=(const ChunkedFile&);
void open(std::string const& filename, std::string const& mode);
void clearUnused();
private:
std::string filename_; //!< path to file
FILE* file_; //!< file pointer
uint64_t offset_; //!< current position in the file
uint64_t compressed_in_; //!< number of bytes written to current compressed stream
char* unused_; //!< extra data read by compressed stream
int nUnused_; //!< number of bytes of extra data read by compressed stream
boost::shared_ptr<StreamFactory> stream_factory_;
boost::shared_ptr<Stream> read_stream_;
boost::shared_ptr<Stream> write_stream_;
};
inline void swap(ChunkedFile& a, ChunkedFile& b) {
a.swap(b);
}
} // namespace rosbag
#endif

62
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/console_bridge_compatibility.h vendored Normal file
View File

@@ -0,0 +1,62 @@
/*
* Copyright (c) 2017, Open Source Robotics Foundation, 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 the Open Source Robotics Foundation, 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__CONSOLE_BRIDGE_COMPATIBILITY_H_
#define ROSBAG__CONSOLE_BRIDGE_COMPATIBILITY_H_
#include <console_bridge/console.h>
// Remove this file and it's inclusions when no longer supporting platforms with
// libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#ifndef CONSOLE_BRIDGE_logError
# define CONSOLE_BRIDGE_logError(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_ERROR, fmt, ##__VA_ARGS__)
#endif
#ifndef CONSOLE_BRIDGE_logWarn
# define CONSOLE_BRIDGE_logWarn(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_WARN, fmt, ##__VA_ARGS__)
#endif
#ifndef CONSOLE_BRIDGE_logInform
# define CONSOLE_BRIDGE_logInform(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_INFO, fmt, ##__VA_ARGS__)
#endif
#ifndef CONSOLE_BRIDGE_logDebug
# define CONSOLE_BRIDGE_logDebug(fmt, ...) \
console_bridge::log( \
__FILE__, __LINE__, console_bridge::CONSOLE_BRIDGE_LOG_DEBUG, fmt, ##__VA_ARGS__)
#endif
#endif // ROSBAG__CONSOLE_BRIDGE_COMPATIBILITY_H_

101
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/constants.h vendored Normal file
View File

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

72
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/exceptions.h vendored Normal file
View File

@@ -0,0 +1,72 @@
/*********************************************************************
* 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_EXCEPTIONS_H
#define ROSBAG_EXCEPTIONS_H
#include <ros/exception.h>
namespace rosbag {
//! Base class for rosbag exceptions
class BagException : public ros::Exception
{
public:
BagException(std::string const& msg) : ros::Exception(msg) { }
};
//! Exception thrown when on IO problems
class BagIOException : public BagException
{
public:
BagIOException(std::string const& msg) : BagException(msg) { }
};
//! Exception thrown on problems reading the bag format
class BagFormatException : public BagException
{
public:
BagFormatException(std::string const& msg) : BagException(msg) { }
};
//! Exception thrown on problems reading the bag index
class BagUnindexedException : public BagException
{
public:
BagUnindexedException() : BagException("Bag unindexed") { }
};
} // namespace rosbag
#endif

45
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/macros.h vendored Normal file
View File

@@ -0,0 +1,45 @@
/*
* Copyright (C) 2008, Willow Garage, Inc.
*
* 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 names of Stanford University or 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_MACROS_H_
#define ROSBAG_MACROS_H_
#include <ros/macros.h> // for the DECL's
// Import/export for windows dll's and visibility for gcc shared libraries.
#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
#ifdef rosbag_EXPORTS // we are building a shared lib/dll
#define ROSBAG_DECL ROS_HELPER_EXPORT
#else // we are using shared lib/dll
#define ROSBAG_DECL ROS_HELPER_IMPORT
#endif
#else // ros is being built around static libraries
#define ROSBAG_DECL
#endif
#endif /* ROSBAG_MACROS_H_ */

175
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/message_instance.h vendored Normal file
View File

@@ -0,0 +1,175 @@
/*********************************************************************
* 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_MESSAGE_INSTANCE_H
#define ROSBAG_MESSAGE_INSTANCE_H
#include <ros/message_traits.h>
#include <ros/serialization.h>
//#include <ros/ros.h>
#include <ros/time.h>
#include "rosbag/structures.h"
#include "rosbag/macros.h"
namespace rosbag {
class Bag;
//! A class pointing into a bag file
/*!
* The MessageInstance class itself is fairly light weight. It
* simply contains a pointer to a bag-file and the index_entry
* necessary to get access to the corresponding data.
*
* It adheres to the necessary ros::message_traits to be directly
* serializable.
*/
class ROSBAG_DECL MessageInstance
{
friend class View;
public:
ros::Time const& getTime() const;
std::string const& getTopic() const;
std::string const& getDataType() const;
std::string const& getMD5Sum() const;
std::string const& getMessageDefinition() const;
boost::shared_ptr<ros::M_string> getConnectionHeader() const;
std::string getCallerId() const;
bool isLatching() const;
//! Test whether the underlying message of the specified type.
/*!
* returns true iff the message is of the template type
*/
template<class T>
bool isType() const;
//! Templated call to instantiate a message
/*!
* returns NULL pointer if incompatible
*/
template<class T>
boost::shared_ptr<T> instantiate() const;
//! Write serialized message contents out to a stream
template<typename Stream>
void write(Stream& stream) const;
//! Size of serialized message
uint32_t size() const;
private:
MessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag);
ConnectionInfo const* connection_info_;
IndexEntry const index_entry_;
Bag const* bag_;
};
} // namespace rosbag
namespace ros {
namespace message_traits {
template<>
struct MD5Sum<rosbag::MessageInstance>
{
static const char* value(const rosbag::MessageInstance& m) { return m.getMD5Sum().c_str(); }
};
template<>
struct DataType<rosbag::MessageInstance>
{
static const char* value(const rosbag::MessageInstance& m) { return m.getDataType().c_str(); }
};
template<>
struct Definition<rosbag::MessageInstance>
{
static const char* value(const rosbag::MessageInstance& m) { return m.getMessageDefinition().c_str(); }
};
} // namespace message_traits
namespace serialization
{
template<>
struct Serializer<rosbag::MessageInstance>
{
template<typename Stream>
inline static void write(Stream& stream, const rosbag::MessageInstance& m) {
m.write(stream);
}
inline static uint32_t serializedLength(const rosbag::MessageInstance& m) {
return m.size();
}
};
} // namespace serialization
} // namespace ros
#include "rosbag/bag.h"
namespace rosbag {
template<class T>
bool MessageInstance::isType() const {
char const* md5sum = ros::message_traits::MD5Sum<T>::value();
return md5sum == std::string("*") || md5sum == getMD5Sum();
}
template<class T>
boost::shared_ptr<T> MessageInstance::instantiate() const {
if (!isType<T>())
return boost::shared_ptr<T>();
return bag_->instantiateBuffer<T>(index_entry_);
}
template<typename Stream>
void MessageInstance::write(Stream& stream) const {
bag_->readMessageDataIntoStream(index_entry_, stream);
}
} // namespace rosbag
#endif

138
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/query.h vendored Normal file
View File

@@ -0,0 +1,138 @@
/*********************************************************************
* 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_QUERY_H
#define ROSBAG_QUERY_H
#include "ros/time.h"
#include <vector>
#include <map>
#include <set>
#include <boost/function.hpp>
#include "rosbag/macros.h"
#include "rosbag/structures.h"
namespace rosbag {
class Bag;
class ROSBAG_DECL Query
{
public:
//! The base query takes an optional time-range
/*!
* param start_time the beginning of the time_range for the query
* param end_time the end of the time_range for the query
*/
Query(boost::function<bool(ConnectionInfo const*)>& query,
ros::Time const& start_time = ros::TIME_MIN,
ros::Time const& end_time = ros::TIME_MAX);
boost::function<bool(ConnectionInfo const*)> const& getQuery() const; //!< Get the query functor
ros::Time const& getStartTime() const; //!< Get the start-time
ros::Time const& getEndTime() const; //!< Get the end-time
private:
boost::function<bool(ConnectionInfo const*)> query_;
ros::Time start_time_;
ros::Time end_time_;
};
class ROSBAG_DECL TopicQuery
{
public:
TopicQuery(std::string const& topic);
TopicQuery(std::vector<std::string> const& topics);
bool operator()(ConnectionInfo const*) const;
private:
std::vector<std::string> topics_;
};
class ROSBAG_DECL TypeQuery
{
public:
TypeQuery(std::string const& type);
TypeQuery(std::vector<std::string> const& types);
bool operator()(ConnectionInfo const*) const;
private:
std::vector<std::string> types_;
};
//! Pairs of queries and the bags they come from (used internally by View)
struct ROSBAG_DECL BagQuery
{
BagQuery(Bag const* _bag, Query const& _query, uint32_t _bag_revision);
Bag const* bag;
Query query;
uint32_t bag_revision;
};
struct ROSBAG_DECL MessageRange
{
MessageRange(std::multiset<IndexEntry>::const_iterator const& _begin,
std::multiset<IndexEntry>::const_iterator const& _end,
ConnectionInfo const* _connection_info,
BagQuery const* _bag_query);
std::multiset<IndexEntry>::const_iterator begin;
std::multiset<IndexEntry>::const_iterator end;
ConnectionInfo const* connection_info;
BagQuery const* bag_query; //!< pointer to vector of queries in View
};
//! The actual iterator data structure
struct ROSBAG_DECL ViewIterHelper
{
ViewIterHelper(std::multiset<IndexEntry>::const_iterator _iter, MessageRange const* _range);
std::multiset<IndexEntry>::const_iterator iter;
MessageRange const* range; //!< pointer to vector of ranges in View
};
struct ROSBAG_DECL ViewIterHelperCompare
{
bool operator()(ViewIterHelper const& a, ViewIterHelper const& b);
};
} // namespace rosbag
#endif

200
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/stream.h vendored Normal file
View File

@@ -0,0 +1,200 @@
/*********************************************************************
* 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_STREAM_H
#define ROSBAG_STREAM_H
#include <ios>
#include <stdint.h>
#include <string>
#include <boost/shared_ptr.hpp>
#include <bzlib.h>
#include <roslz4/lz4s.h>
#include "rosbag/exceptions.h"
#include "rosbag/macros.h"
namespace rosbag {
namespace compression
{
enum CompressionType
{
Uncompressed = 0,
BZ2 = 1,
LZ4 = 2,
};
}
typedef compression::CompressionType CompressionType;
class ChunkedFile;
class FileAccessor;
class ROSBAG_DECL Stream
{
friend class FileAccessor;
public:
Stream(ChunkedFile* file);
virtual ~Stream();
virtual CompressionType getCompressionType() const = 0;
virtual void write(void* ptr, size_t size) = 0;
virtual void read (void* ptr, size_t size) = 0;
virtual void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) = 0;
virtual void startWrite();
virtual void stopWrite();
virtual void startRead();
virtual void stopRead();
protected:
FILE* getFilePointer();
uint64_t getCompressedIn();
void setCompressedIn(uint64_t nbytes);
void advanceOffset(uint64_t nbytes);
char* getUnused();
int getUnusedLength();
void setUnused(char* unused);
void setUnusedLength(int nUnused);
void clearUnused();
protected:
ChunkedFile* file_;
};
class ROSBAG_DECL StreamFactory
{
public:
StreamFactory(ChunkedFile* file);
boost::shared_ptr<Stream> getStream(CompressionType type) const;
private:
boost::shared_ptr<Stream> uncompressed_stream_;
boost::shared_ptr<Stream> bz2_stream_;
boost::shared_ptr<Stream> lz4_stream_;
};
class FileAccessor {
friend class ChunkedFile;
static void setFile(Stream& a, ChunkedFile* file) {
a.file_ = file;
}
};
class ROSBAG_DECL UncompressedStream : public Stream
{
public:
UncompressedStream(ChunkedFile* file);
CompressionType getCompressionType() const;
void write(void* ptr, size_t size);
void read(void* ptr, size_t size);
void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
};
/*!
* BZ2Stream uses libbzip2 (http://www.bzip.org) for reading/writing compressed data in the BZ2 format.
*/
class ROSBAG_DECL BZ2Stream : public Stream
{
public:
BZ2Stream(ChunkedFile* file);
CompressionType getCompressionType() const;
void startWrite();
void write(void* ptr, size_t size);
void stopWrite();
void startRead();
void read(void* ptr, size_t size);
void stopRead();
void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
private:
int verbosity_; //!< level of debugging output (0-4; 0 default). 0 is silent, 4 is max verbose debugging output
int block_size_100k_; //!< compression block size (1-9; 9 default). 9 is best compression, most memory
int work_factor_; //!< compression behavior for worst case, highly repetitive data (0-250; 30 default)
BZFILE* bzfile_; //!< bzlib compressed file stream
int bzerror_; //!< last error from bzlib
};
// LZ4Stream reads/writes compressed datat in the LZ4 format
// https://code.google.com/p/lz4/
class ROSBAG_DECL LZ4Stream : public Stream
{
public:
LZ4Stream(ChunkedFile* file);
~LZ4Stream();
CompressionType getCompressionType() const;
void startWrite();
void write(void* ptr, size_t size);
void stopWrite();
void startRead();
void read(void* ptr, size_t size);
void stopRead();
void decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len);
private:
LZ4Stream(const LZ4Stream&);
LZ4Stream operator=(const LZ4Stream&);
void writeStream(int action);
char *buff_;
int buff_size_;
int block_size_id_;
roslz4_stream lz4s_;
};
} // namespace rosbag
#endif

93
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/structures.h vendored Normal file
View File

@@ -0,0 +1,93 @@
/*********************************************************************
* 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_STRUCTURES_H
#define ROSBAG_STRUCTURES_H
#include <map>
#include <vector>
#include "ros/time.h"
#include "ros/datatypes.h"
#include "macros.h"
namespace rosbag {
struct ROSBAG_DECL ConnectionInfo
{
ConnectionInfo() : id(-1) { }
uint32_t id;
std::string topic;
std::string datatype;
std::string md5sum;
std::string msg_def;
boost::shared_ptr<ros::M_string> header;
};
struct ChunkInfo
{
ros::Time start_time; //! earliest timestamp of a message in the chunk
ros::Time end_time; //! latest timestamp of a message in the chunk
uint64_t pos; //! absolute byte offset of chunk record in bag file
std::map<uint32_t, uint32_t> connection_counts; //! number of messages in each connection stored in the chunk
};
struct ROSBAG_DECL ChunkHeader
{
std::string compression; //! chunk compression type, e.g. "none" or "bz2" (see constants.h)
uint32_t compressed_size; //! compressed size of the chunk in bytes
uint32_t uncompressed_size; //! uncompressed size of the chunk in bytes
};
struct ROSBAG_DECL IndexEntry
{
ros::Time time; //! timestamp of the message
uint64_t chunk_pos; //! absolute byte offset of the chunk record containing the message
uint32_t offset; //! relative byte offset of the message record (either definition or data) in the chunk
bool operator<(IndexEntry const& b) const { return time < b.time; }
};
struct ROSBAG_DECL IndexEntryCompare
{
bool operator()(ros::Time const& a, IndexEntry const& b) const { return a < b.time; }
bool operator()(IndexEntry const& a, ros::Time const& b) const { return a.time < b; }
};
} // namespace rosbag
#endif

179
thirdparty/ros/ros_comm/tools/rosbag_storage/include/rosbag/view.h vendored Normal file
View File

@@ -0,0 +1,179 @@
/*********************************************************************
* 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_VIEW_H
#define ROSBAG_VIEW_H
#include <boost/function.hpp>
#include <boost/iterator/iterator_facade.hpp>
#include "rosbag/message_instance.h"
#include "rosbag/query.h"
#include "rosbag/macros.h"
#include "rosbag/structures.h"
namespace rosbag {
class ROSBAG_DECL View
{
friend class Bag;
public:
//! An iterator that points to a MessageInstance from a bag
/*!
* This iterator stores the MessageInstance that it is returning a
* reference to. If you increment the iterator that
* MessageInstance is destroyed. You should never store the
* pointer to this reference.
*/
class iterator : public boost::iterator_facade<iterator,
MessageInstance,
boost::forward_traversal_tag>
{
public:
iterator(iterator const& i);
iterator &operator=(iterator const& i);
iterator();
~iterator();
protected:
iterator(View* view, bool end = false);
private:
friend class View;
friend class boost::iterator_core_access;
void populate();
void populateSeek(std::multiset<IndexEntry>::const_iterator iter);
bool equal(iterator const& other) const;
void increment();
MessageInstance& dereference() const;
private:
View* view_;
std::vector<ViewIterHelper> iters_;
uint32_t view_revision_;
mutable MessageInstance* message_instance_;
};
typedef iterator const_iterator;
struct TrueQuery {
bool operator()(ConnectionInfo const*) const { return true; };
};
//! Create a view on a bag
/*!
* param reduce_overlap If multiple views return the same messages, reduce them to a single message
*/
View(bool const& reduce_overlap = false);
//! Create a view on a bag
/*!
* param bag The bag file on which to run this query
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
* param reduce_overlap If multiple views return the same messages, reduce them to a single message
*/
View(Bag const& bag, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);
//! Create a view and add a query
/*!
* param bag The bag file on which to run this query
* param query The actual query to evaluate which connections to include
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
* param reduce_overlap If multiple views return the same messages, reduce them to a single message
*/
View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX, bool const& reduce_overlap = false);
~View();
iterator begin();
iterator end();
uint32_t size();
//! Add a query to a view
/*!
* param bag The bag file on which to run this query
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
*/
void addQuery(Bag const& bag, ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX);
//! Add a query to a view
/*!
* param bag The bag file on which to run this query
* param query The actual query to evaluate which connections to include
* param start_time The beginning of the time range for the query
* param end_time The end of the time range for the query
*/
void addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query,
ros::Time const& start_time = ros::TIME_MIN, ros::Time const& end_time = ros::TIME_MAX);
std::vector<const ConnectionInfo*> getConnections();
ros::Time getBeginTime();
ros::Time getEndTime();
protected:
friend class iterator;
void updateQueries(BagQuery* q);
void update();
MessageInstance* newMessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag);
private:
View(View const& view);
View& operator=(View const& view);
protected:
std::vector<MessageRange*> ranges_;
std::vector<BagQuery*> queries_;
uint32_t view_revision_;
uint32_t size_cache_;
uint32_t size_revision_;
bool reduce_overlap_;
};
} // namespace rosbag
#endif

66
thirdparty/ros/ros_comm/tools/rosbag_storage/mainpage.dox vendored Normal file
View File

@@ -0,0 +1,66 @@
/**
\mainpage
\htmlinclude manifest.html
\b rosbag_storage is a set of tools and API's for recording/writing messages to bag files and playing/reading them back without relying on the ROS client library.
The code is still in the rosbag namespace since it was extracted from the <a href="../../../rosbag/html/c++/">rosbag</a> package without renaming any API.
\section codeapi Code API
The C++ and Python API's are provided for serializing bag files. The C++ API consists of the following classes:
- rosbag::Bag - Serializes to/from a bag file on disk.
- rosbag::View - Specifies a view into a bag file to allow for querying for messages on specific connections withn a time range.
Here's a simple example of writing to a bag file:
\verbatim
#include "rosbag/bag.h"
...
rosbag::Bag bag("test.bag", rosbag::bagmode::Write);
std_msgs::Int32 i;
i.data = 42;
bag.write("numbers", ros::Time::now(), i);
bag.close();
\endverbatim
Likewise, to read from that bag file:
\verbatim
#include "rosbag/bag.h"
...
rosbag::Bag bag("test.bag");
rosbag::View view(bag, rosbag::TopicQuery("numbers"));
BOOST_FOREACH(rosbag::MessageInstance const m, view)
{
std_msgs::Int32::ConstPtr i = m.instantiate<std_msgs::Int32>();
if (i != NULL)
std::cout << i->data << std::endl;
}
bag.close();
\endverbatim
The Python API is similar. Writing to a bag file:
\verbatim
import rosbag
from std_msgs.msg import Int32, String
bag = rosbag.Bag('test.bag', 'w')
i = Int32()
i.data = 42
bag.write('numbers', i);
bag.close();
\endverbatim
Example usage for read:
\verbatim
import rosbag
bag = rosbag.Bag('test.bag')
for topic, msg, t in bag.read_messages('numbers'):
print msg.data
bag.close();
\endverbatim
*/

34
thirdparty/ros/ros_comm/tools/rosbag_storage/package.xml vendored Normal file
View File

@@ -0,0 +1,34 @@
<package>
<name>rosbag_storage</name>
<version>1.12.14</version>
<description>
This is a set of tools for recording from and playing back ROS
message without relying on the ROS client library.
</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>boost</build_depend>
<build_depend>bzip2</build_depend>
<build_depend version_gte="0.3.17">cpp_common</build_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>roscpp_serialization</build_depend>
<build_depend version_gte="0.3.17">roscpp_traits</build_depend>
<build_depend>rostime</build_depend>
<build_depend>roslz4</build_depend>
<run_depend>boost</run_depend>
<run_depend>bzip2</run_depend>
<run_depend version_gte="0.3.17">cpp_common</run_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<run_depend>roscpp_serialization</run_depend>
<run_depend version_gte="0.3.17">roscpp_traits</run_depend>
<run_depend>rostime</run_depend>
<run_depend>roslz4</run_depend>
<export>
<rosdoc config="${prefix}/rosdoc.yaml"/>
</export>
</package>

4
thirdparty/ros/ros_comm/tools/rosbag_storage/rosdoc.yaml vendored Normal file
View File

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

1154
thirdparty/ros/ros_comm/tools/rosbag_storage/src/bag.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

71
thirdparty/ros/ros_comm/tools/rosbag_storage/src/bag_player.cpp vendored Normal file
View File

@@ -0,0 +1,71 @@
#include "rosbag/bag_player.h"
#define foreach BOOST_FOREACH
namespace rosbag
{
BagPlayer::BagPlayer(const std::string &fname) {
bag.open(fname, rosbag::bagmode::Read);
ros::Time::init();
View v(bag);
bag_start_ = v.getBeginTime();
bag_end_ = v.getEndTime();
last_message_time_ = ros::Time(0);
playback_speed_ = 1.0;
}
BagPlayer::~BagPlayer() {
bag.close();
}
ros::Time BagPlayer::get_time() {
return last_message_time_;
}
void BagPlayer::set_start(const ros::Time &start) {
bag_start_ = start;
}
void BagPlayer::set_end(const ros::Time &end) {
bag_end_ = end;
}
void BagPlayer::set_playback_speed(double scale) {
if (scale > 0.0)
playback_speed_ = scale;
}
ros::Time BagPlayer::real_time(const ros::Time &msg_time) {
return play_start_ + (msg_time - bag_start_) * (1 / playback_speed_);
}
void BagPlayer::start_play() {
std::vector<std::string> topics;
std::pair<std::string, BagCallback *> cb;
foreach(cb, cbs_)
topics.push_back(cb.first);
View view(bag, TopicQuery(topics), bag_start_, bag_end_);
play_start_ = ros::Time::now();
foreach(MessageInstance const m, view)
{
if (cbs_.find(m.getTopic()) == cbs_.end())
continue;
ros::Time::sleepUntil(real_time(m.getTime()));
last_message_time_ = m.getTime(); /* this is the recorded time */
cbs_[m.getTopic()]->call(m);
}
}
void BagPlayer::unregister_callback(const std::string &topic) {
delete cbs_[topic];
cbs_.erase(topic);
}
}

82
thirdparty/ros/ros_comm/tools/rosbag_storage/src/buffer.cpp vendored Normal file
View File

@@ -0,0 +1,82 @@
/*********************************************************************
* 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 <stdlib.h>
#include <assert.h>
#include <utility>
#include "rosbag/buffer.h"
//#include <ros/ros.h>
namespace rosbag {
Buffer::Buffer() : buffer_(NULL), capacity_(0), size_(0) { }
Buffer::~Buffer() {
free(buffer_);
}
uint8_t* Buffer::getData() { return buffer_; }
uint32_t Buffer::getCapacity() const { return capacity_; }
uint32_t Buffer::getSize() const { return size_; }
void Buffer::setSize(uint32_t size) {
size_ = size;
ensureCapacity(size);
}
void Buffer::ensureCapacity(uint32_t capacity) {
if (capacity <= capacity_)
return;
if (capacity_ == 0)
capacity_ = capacity;
else {
while (capacity_ < capacity)
capacity_ *= 2;
}
buffer_ = (uint8_t*) realloc(buffer_, capacity_);
assert(buffer_);
}
void Buffer::swap(Buffer& other) {
using std::swap;
swap(buffer_, other.buffer_);
swap(capacity_, other.capacity_);
swap(size_, other.size_);
}
} // namespace rosbag

178
thirdparty/ros/ros_comm/tools/rosbag_storage/src/bz2_stream.cpp vendored Normal file
View File

@@ -0,0 +1,178 @@
/*********************************************************************
* 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/chunked_file.h"
#include <iostream>
#include <cstring>
#include "console_bridge/console.h"
// Remove this include when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#include "rosbag/console_bridge_compatibility.h"
using std::string;
namespace rosbag {
BZ2Stream::BZ2Stream(ChunkedFile* file) :
Stream(file),
verbosity_(0),
block_size_100k_(9),
work_factor_(30),
bzfile_(NULL),
bzerror_(0)
{ }
CompressionType BZ2Stream::getCompressionType() const {
return compression::BZ2;
}
void BZ2Stream::startWrite() {
bzfile_ = BZ2_bzWriteOpen(&bzerror_, getFilePointer(), block_size_100k_, verbosity_, work_factor_);
switch (bzerror_) {
case BZ_OK: break;
default: {
BZ2_bzWriteClose(&bzerror_, bzfile_, 0, NULL, NULL);
throw BagException("Error opening file for writing compressed stream");
}
}
setCompressedIn(0);
}
void BZ2Stream::write(void* ptr, size_t size) {
if (!bzfile_) {
throw BagException("cannot write to unopened bzfile");
}
BZ2_bzWrite(&bzerror_, bzfile_, ptr, size);
switch (bzerror_) {
case BZ_IO_ERROR: throw BagException("BZ_IO_ERROR: error writing the compressed file");
}
setCompressedIn(getCompressedIn() + size);
}
void BZ2Stream::stopWrite() {
if (!bzfile_) {
throw BagException("cannot close unopened bzfile");
}
unsigned int nbytes_in;
unsigned int nbytes_out;
BZ2_bzWriteClose(&bzerror_, bzfile_, 0, &nbytes_in, &nbytes_out);
switch (bzerror_) {
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR");
}
advanceOffset(nbytes_out);
setCompressedIn(0);
}
void BZ2Stream::startRead() {
bzfile_ = BZ2_bzReadOpen(&bzerror_, getFilePointer(), verbosity_, 0, getUnused(), getUnusedLength());
switch (bzerror_) {
case BZ_OK: break;
default: {
BZ2_bzReadClose(&bzerror_, bzfile_);
throw BagException("Error opening file for reading compressed stream");
}
}
clearUnused();
}
void BZ2Stream::read(void* ptr, size_t size) {
if (!bzfile_) {
throw BagException("cannot read from unopened bzfile");
}
BZ2_bzRead(&bzerror_, bzfile_, ptr, size);
advanceOffset(size);
switch (bzerror_) {
case BZ_OK: return;
case BZ_STREAM_END:
if (getUnused() || getUnusedLength() > 0)
CONSOLE_BRIDGE_logError("unused data already available");
else {
char* unused;
int nUnused;
BZ2_bzReadGetUnused(&bzerror_, bzfile_, (void**) &unused, &nUnused);
setUnused(unused);
setUnusedLength(nUnused);
}
return;
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR: error reading from compressed stream"); break;
case BZ_UNEXPECTED_EOF: throw BagIOException("BZ_UNEXPECTED_EOF: compressed stream ended before logical end-of-stream detected"); break;
case BZ_DATA_ERROR: throw BagIOException("BZ_DATA_ERROR: data integrity error detected in compressed stream"); break;
case BZ_DATA_ERROR_MAGIC: throw BagIOException("BZ_DATA_ERROR_MAGIC: stream does not begin with requisite header bytes"); break;
case BZ_MEM_ERROR: throw BagIOException("BZ_MEM_ERROR: insufficient memory available"); break;
}
}
void BZ2Stream::stopRead() {
if (!bzfile_) {
throw BagException("cannot close unopened bzfile");
}
BZ2_bzReadClose(&bzerror_, bzfile_);
switch (bzerror_) {
case BZ_IO_ERROR: throw BagIOException("BZ_IO_ERROR");
}
}
void BZ2Stream::decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
int result = BZ2_bzBuffToBuffDecompress((char*) dest, &dest_len, (char*) source, source_len, 0, verbosity_);
switch (result) {
case BZ_OK: break;
case BZ_CONFIG_ERROR: throw BagException("library has been mis-compiled"); break;
case BZ_PARAM_ERROR: throw BagException("dest is NULL or destLen is NULL or small != 0 && small != 1 or verbosity < 0 or verbosity > 4"); break;
case BZ_MEM_ERROR: throw BagException("insufficient memory is available"); break;
case BZ_OUTBUFF_FULL: throw BagException("size of the compressed data exceeds *destLen"); break;
case BZ_DATA_ERROR: throw BagException("data integrity error was detected in the compressed data"); break;
case BZ_DATA_ERROR_MAGIC: throw BagException("compressed data doesn't begin with the right magic bytes"); break;
case BZ_UNEXPECTED_EOF: throw BagException("compressed data ends unexpectedly"); break;
}
}
} // namespace rosbag

255
thirdparty/ros/ros_comm/tools/rosbag_storage/src/chunked_file.cpp vendored Normal file
View File

@@ -0,0 +1,255 @@
/*********************************************************************
* 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/chunked_file.h"
#include <iostream>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>
//#include <ros/ros.h>
#ifdef _WIN32
# ifdef __MINGW32__
# define fseeko fseeko64
# define ftello ftello64
// not sure if we need a ftruncate here yet or not
# else
# include <io.h>
# define fseeko _fseeki64
# define ftello _ftelli64
# define fileno _fileno
# define ftruncate _chsize
# endif
#endif
using std::string;
using boost::format;
using boost::shared_ptr;
using ros::Exception;
namespace rosbag {
ChunkedFile::ChunkedFile() :
file_(NULL),
offset_(0),
compressed_in_(0),
unused_(NULL),
nUnused_(0)
{
stream_factory_ = boost::make_shared<StreamFactory>(this);
}
ChunkedFile::~ChunkedFile() {
close();
}
void ChunkedFile::openReadWrite(string const& filename) { open(filename, "r+b"); }
void ChunkedFile::openWrite (string const& filename) { open(filename, "w+b"); }
void ChunkedFile::openRead (string const& filename) { open(filename, "rb"); }
void ChunkedFile::open(string const& filename, string const& mode) {
// Check if file is already open
if (file_)
throw BagIOException((format("File already open: %1%") % filename_.c_str()).str());
// Open the file
if (mode == "r+b") {
// check if file already exists
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), "r" );
#else
file_ = fopen(filename.c_str(), "r");
#endif
if (file_ == NULL)
// create an empty file and open it for update
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), "w+b" );
#else
file_ = fopen(filename.c_str(), "w+b");
#endif
else {
fclose(file_);
// open existing file for update
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), "r+b" );
#else
file_ = fopen(filename.c_str(), "r+b");
#endif
}
}
else
#if defined(_MSC_VER) && (_MSC_VER >= 1400 )
fopen_s( &file_, filename.c_str(), mode.c_str() );
#else
file_ = fopen(filename.c_str(), mode.c_str());
#endif
if (!file_)
throw BagIOException((format("Error opening file: %1%") % filename.c_str()).str());
read_stream_ = boost::make_shared<UncompressedStream>(this);
write_stream_ = boost::make_shared<UncompressedStream>(this);
filename_ = filename;
offset_ = ftello(file_);
}
bool ChunkedFile::good() const {
return feof(file_) == 0 && ferror(file_) == 0;
}
bool ChunkedFile::isOpen() const { return file_ != NULL; }
string ChunkedFile::getFileName() const { return filename_; }
void ChunkedFile::close() {
if (!file_)
return;
// Close any compressed stream by changing to uncompressed mode
setWriteMode(compression::Uncompressed);
// Close the file
int success = fclose(file_);
if (success != 0)
throw BagIOException((format("Error closing file: %1%") % filename_.c_str()).str());
file_ = NULL;
filename_.clear();
clearUnused();
}
// Read/write modes
void ChunkedFile::setWriteMode(CompressionType type) {
if (!file_)
throw BagIOException("Can't set compression mode before opening a file");
if (type != write_stream_->getCompressionType()) {
write_stream_->stopWrite();
shared_ptr<Stream> stream = stream_factory_->getStream(type);
stream->startWrite();
write_stream_ = stream;
}
}
void ChunkedFile::setReadMode(CompressionType type) {
if (!file_)
throw BagIOException("Can't set compression mode before opening a file");
if (type != read_stream_->getCompressionType()) {
read_stream_->stopRead();
shared_ptr<Stream> stream = stream_factory_->getStream(type);
stream->startRead();
read_stream_ = stream;
}
}
void ChunkedFile::seek(uint64_t offset, int origin) {
if (!file_)
throw BagIOException("Can't seek - file not open");
setReadMode(compression::Uncompressed);
int success = fseeko(file_, offset, origin);
if (success != 0)
throw BagIOException("Error seeking");
offset_ = ftello(file_);
}
uint64_t ChunkedFile::getOffset() const { return offset_; }
uint32_t ChunkedFile::getCompressedBytesIn() const { return compressed_in_; }
void ChunkedFile::write(string const& s) { write((void*) s.c_str(), s.size()); }
void ChunkedFile::write(void* ptr, size_t size) { write_stream_->write(ptr, size); }
void ChunkedFile::read(void* ptr, size_t size) { read_stream_->read(ptr, size); }
bool ChunkedFile::truncate(uint64_t length) {
int fd = fileno(file_);
return ftruncate(fd, length) == 0;
}
//! \todo add error handling
string ChunkedFile::getline() {
char buffer[1024];
if(fgets(buffer, 1024, file_))
{
string s(buffer);
offset_ += s.size();
return s;
}
else
return string("");
}
void ChunkedFile::decompress(CompressionType compression, uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
stream_factory_->getStream(compression)->decompress(dest, dest_len, source, source_len);
}
void ChunkedFile::clearUnused() {
unused_ = NULL;
nUnused_ = 0;
}
void ChunkedFile::swap(ChunkedFile& other) {
using std::swap;
using boost::swap;
swap(filename_, other.filename_);
swap(file_, other.file_);
swap(offset_, other.offset_);
swap(compressed_in_, other.compressed_in_);
swap(unused_, other.unused_);
swap(nUnused_, other.nUnused_);
swap(stream_factory_, other.stream_factory_);
FileAccessor::setFile(*stream_factory_->getStream(compression::Uncompressed), this);
FileAccessor::setFile(*stream_factory_->getStream(compression::BZ2), this);
FileAccessor::setFile(*stream_factory_->getStream(compression::LZ4), this);
FileAccessor::setFile(*other.stream_factory_->getStream(compression::Uncompressed), &other);
FileAccessor::setFile(*other.stream_factory_->getStream(compression::BZ2), &other);
FileAccessor::setFile(*other.stream_factory_->getStream(compression::LZ4), &other);
swap(read_stream_, other.read_stream_);
FileAccessor::setFile(*read_stream_, this);
FileAccessor::setFile(*other.read_stream_, &other);
swap(write_stream_, other.write_stream_);
FileAccessor::setFile(*write_stream_, this);
FileAccessor::setFile(*other.write_stream_, &other);
}
} // namespace rosbag

233
thirdparty/ros/ros_comm/tools/rosbag_storage/src/lz4_stream.cpp vendored Normal file
View File

@@ -0,0 +1,233 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2014, Ben Charrow
* 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/chunked_file.h"
#include <iostream>
#include <cstring>
#include "console_bridge/console.h"
// Remove this include when no longer supporting platforms with libconsole-bridge-dev < 0.3.0,
// in particular Debian Jessie: https://packages.debian.org/jessie/libconsole-bridge-dev
#include "rosbag/console_bridge_compatibility.h"
using std::string;
namespace rosbag {
LZ4Stream::LZ4Stream(ChunkedFile* file)
: Stream(file), block_size_id_(6) {
buff_size_ = roslz4_blockSizeFromIndex(block_size_id_) + 64;
buff_ = new char[buff_size_];
lz4s_.state = NULL;
}
LZ4Stream::~LZ4Stream() {
delete[] buff_;
}
CompressionType LZ4Stream::getCompressionType() const {
return compression::LZ4;
}
void LZ4Stream::startWrite() {
if (lz4s_.state) {
throw BagException("cannot start writing to already opened lz4 stream");
}
setCompressedIn(0);
int ret = roslz4_compressStart(&lz4s_, block_size_id_);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_MEMORY_ERROR: throw BagIOException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
case ROSLZ4_PARAM_ERROR: throw BagIOException("ROSLZ4_PARAM_ERROR: bad block size"); break;
default: throw BagException("Unhandled return code");
}
lz4s_.output_next = buff_;
lz4s_.output_left = buff_size_;
}
void LZ4Stream::write(void* ptr, size_t size) {
if (!lz4s_.state) {
throw BagException("cannot write to unopened lz4 stream");
}
lz4s_.input_left = size;
lz4s_.input_next = (char*) ptr;
writeStream(ROSLZ4_RUN);
setCompressedIn(getCompressedIn() + size);
}
void LZ4Stream::writeStream(int action) {
int ret = ROSLZ4_OK;
while (lz4s_.input_left > 0 ||
(action == ROSLZ4_FINISH && ret != ROSLZ4_STREAM_END)) {
ret = roslz4_compress(&lz4s_, action);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_OUTPUT_SMALL:
if (lz4s_.output_next - buff_ == buff_size_) {
throw BagIOException("ROSLZ4_OUTPUT_SMALL: output buffer is too small");
} else {
// There's data to be written in buff_; this will free up space
break;
}
case ROSLZ4_STREAM_END: break;
case ROSLZ4_PARAM_ERROR: throw BagIOException("ROSLZ4_PARAM_ERROR: bad block size"); break;
case ROSLZ4_ERROR: throw BagIOException("ROSLZ4_ERROR: compression error"); break;
default: throw BagException("Unhandled return code");
}
// If output data is ready, write to disk
int to_write = lz4s_.output_next - buff_;
if (to_write > 0) {
if (fwrite(buff_, 1, to_write, getFilePointer()) != static_cast<size_t>(to_write)) {
throw BagException("Problem writing data to disk");
}
advanceOffset(to_write);
lz4s_.output_next = buff_;
lz4s_.output_left = buff_size_;
}
}
}
void LZ4Stream::stopWrite() {
if (!lz4s_.state) {
throw BagException("cannot close unopened lz4 stream");
}
writeStream(ROSLZ4_FINISH);
setCompressedIn(0);
roslz4_compressEnd(&lz4s_);
}
void LZ4Stream::startRead() {
if (lz4s_.state) {
throw BagException("cannot start reading from already opened lz4 stream");
}
int ret = roslz4_decompressStart(&lz4s_);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_MEMORY_ERROR: throw BagException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
default: throw BagException("Unhandled return code");
}
if (getUnusedLength() > buff_size_) {
throw BagException("Too many unused bytes to decompress");
}
// getUnused() could be pointing to part of buff_, so don't use memcpy
memmove(buff_, getUnused(), getUnusedLength());
lz4s_.input_next = buff_;
lz4s_.input_left = getUnusedLength();
clearUnused();
}
void LZ4Stream::read(void* ptr, size_t size) {
if (!lz4s_.state) {
throw BagException("cannot read from unopened lz4 stream");
}
// Setup stream by filling buffer with data from file
int to_read = buff_size_ - lz4s_.input_left;
char *input_start = buff_ + lz4s_.input_left;
int nread = fread(input_start, 1, to_read, getFilePointer());
if (ferror(getFilePointer())) {
throw BagIOException("Problem reading from file");
}
lz4s_.input_next = buff_;
lz4s_.input_left += nread;
lz4s_.output_next = (char*) ptr;
lz4s_.output_left = size;
// Decompress. If reach end of stream, store unused data
int ret = roslz4_decompress(&lz4s_);
switch (ret) {
case ROSLZ4_OK: break;
case ROSLZ4_STREAM_END:
if (getUnused() || getUnusedLength() > 0)
CONSOLE_BRIDGE_logError("unused data already available");
else {
setUnused(lz4s_.input_next);
setUnusedLength(lz4s_.input_left);
}
return;
case ROSLZ4_ERROR: throw BagException("ROSLZ4_ERROR: decompression error"); break;
case ROSLZ4_MEMORY_ERROR: throw BagException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
case ROSLZ4_OUTPUT_SMALL: throw BagException("ROSLZ4_OUTPUT_SMALL: output buffer is too small"); break;
case ROSLZ4_DATA_ERROR: throw BagException("ROSLZ4_DATA_ERROR: malformed data to decompress"); break;
default: throw BagException("Unhandled return code");
}
if (feof(getFilePointer())) {
throw BagIOException("Reached end of file before reaching end of stream");
}
size_t total_out = lz4s_.output_next - (char*)ptr;
advanceOffset(total_out);
// Shift input buffer if there's unconsumed data
if (lz4s_.input_left > 0) {
memmove(buff_, lz4s_.input_next, lz4s_.input_left);
}
}
void LZ4Stream::stopRead() {
if (!lz4s_.state) {
throw BagException("cannot close unopened lz4 stream");
}
roslz4_decompressEnd(&lz4s_);
}
void LZ4Stream::decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
unsigned int actual_dest_len = dest_len;
int ret = roslz4_buffToBuffDecompress((char*)source, source_len,
(char*)dest, &actual_dest_len);
switch(ret) {
case ROSLZ4_OK: break;
case ROSLZ4_ERROR: throw BagException("ROSLZ4_ERROR: decompression error"); break;
case ROSLZ4_MEMORY_ERROR: throw BagException("ROSLZ4_MEMORY_ERROR: insufficient memory available"); break;
case ROSLZ4_OUTPUT_SMALL: throw BagException("ROSLZ4_OUTPUT_SMALL: output buffer is too small"); break;
case ROSLZ4_DATA_ERROR: throw BagException("ROSLZ4_DATA_ERROR: malformed data to decompress"); break;
default: throw BagException("Unhandled return code");
}
if (actual_dest_len != dest_len) {
throw BagException("Decompression size mismatch in LZ4 chunk");
}
}
} // namespace rosbag

65
thirdparty/ros/ros_comm/tools/rosbag_storage/src/message_instance.cpp vendored Normal file
View File

@@ -0,0 +1,65 @@
// 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.
#include "rosbag/message_instance.h"
#include "ros/message_event.h"
using std::string;
using ros::Time;
using boost::shared_ptr;
namespace rosbag {
MessageInstance::MessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index_entry, Bag const& bag) :
connection_info_(connection_info), index_entry_(index_entry), bag_(&bag)
{
}
Time const& MessageInstance::getTime() const { return index_entry_.time; }
string const& MessageInstance::getTopic() const { return connection_info_->topic; }
string const& MessageInstance::getDataType() const { return connection_info_->datatype; }
string const& MessageInstance::getMD5Sum() const { return connection_info_->md5sum; }
string const& MessageInstance::getMessageDefinition() const { return connection_info_->msg_def; }
shared_ptr<ros::M_string> MessageInstance::getConnectionHeader() const { return connection_info_->header; }
string MessageInstance::getCallerId() const {
ros::M_string::const_iterator header_iter = connection_info_->header->find("callerid");
return header_iter != connection_info_->header->end() ? header_iter->second : string("");
}
bool MessageInstance::isLatching() const {
ros::M_string::const_iterator header_iter = connection_info_->header->find("latching");
return header_iter != connection_info_->header->end() && header_iter->second == "1";
}
uint32_t MessageInstance::size() const {
return bag_->readMessageDataSize(index_entry_);
}
} // namespace rosbag

114
thirdparty/ros/ros_comm/tools/rosbag_storage/src/query.cpp vendored Normal file
View File

@@ -0,0 +1,114 @@
// 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.
#include "rosbag/query.h"
#include "rosbag/bag.h"
#include <boost/foreach.hpp>
#define foreach BOOST_FOREACH
using std::map;
using std::string;
using std::vector;
using std::multiset;
namespace rosbag {
// Query
Query::Query(boost::function<bool(ConnectionInfo const*)>& query, ros::Time const& start_time, ros::Time const& end_time)
: query_(query), start_time_(start_time), end_time_(end_time)
{
}
boost::function<bool(ConnectionInfo const*)> const& Query::getQuery() const {
return query_;
}
ros::Time const& Query::getStartTime() const { return start_time_; }
ros::Time const& Query::getEndTime() const { return end_time_; }
// TopicQuery
TopicQuery::TopicQuery(std::string const& topic) {
topics_.push_back(topic);
}
TopicQuery::TopicQuery(std::vector<std::string> const& topics) : topics_(topics) { }
bool TopicQuery::operator()(ConnectionInfo const* info) const {
foreach(string const& topic, topics_)
if (topic == info->topic)
return true;
return false;
}
// TypeQuery
TypeQuery::TypeQuery(std::string const& type) {
types_.push_back(type);
}
TypeQuery::TypeQuery(std::vector<std::string> const& types) : types_(types) { }
bool TypeQuery::operator()(ConnectionInfo const* info) const {
foreach(string const& type, types_)
if (type == info->datatype)
return true;
return false;
}
// BagQuery
BagQuery::BagQuery(Bag const* _bag, Query const& _query, uint32_t _bag_revision) : bag(_bag), query(_query), bag_revision(_bag_revision) {
}
// MessageRange
MessageRange::MessageRange(std::multiset<IndexEntry>::const_iterator const& _begin,
std::multiset<IndexEntry>::const_iterator const& _end,
ConnectionInfo const* _connection_info,
BagQuery const* _bag_query)
: begin(_begin), end(_end), connection_info(_connection_info), bag_query(_bag_query)
{
}
// ViewIterHelper
ViewIterHelper::ViewIterHelper(std::multiset<IndexEntry>::const_iterator _iter, MessageRange const* _range)
: iter(_iter), range(_range)
{
}
bool ViewIterHelperCompare::operator()(ViewIterHelper const& a, ViewIterHelper const& b) {
return (a.iter)->time > (b.iter)->time;
}
} // namespace rosbag

83
thirdparty/ros/ros_comm/tools/rosbag_storage/src/stream.cpp vendored Normal file
View File

@@ -0,0 +1,83 @@
/*********************************************************************
* 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/stream.h"
#include "rosbag/chunked_file.h"
//#include <ros/ros.h>
using boost::shared_ptr;
namespace rosbag {
// StreamFactory
StreamFactory::StreamFactory(ChunkedFile* file) :
uncompressed_stream_(new UncompressedStream(file)),
bz2_stream_ (new BZ2Stream(file)),
lz4_stream_ (new LZ4Stream(file))
{
}
shared_ptr<Stream> StreamFactory::getStream(CompressionType type) const {
switch (type) {
case compression::Uncompressed: return uncompressed_stream_;
case compression::BZ2: return bz2_stream_;
case compression::LZ4: return lz4_stream_;
default: return shared_ptr<Stream>();
}
}
// Stream
Stream::Stream(ChunkedFile* file) : file_(file) { }
Stream::~Stream() { }
void Stream::startWrite() { }
void Stream::stopWrite() { }
void Stream::startRead() { }
void Stream::stopRead() { }
FILE* Stream::getFilePointer() { return file_->file_; }
uint64_t Stream::getCompressedIn() { return file_->compressed_in_; }
void Stream::setCompressedIn(uint64_t nbytes) { file_->compressed_in_ = nbytes; }
void Stream::advanceOffset(uint64_t nbytes) { file_->offset_ += nbytes; }
char* Stream::getUnused() { return file_->unused_; }
int Stream::getUnusedLength() { return file_->nUnused_; }
void Stream::setUnused(char* unused) { file_->unused_ = unused; }
void Stream::setUnusedLength(int nUnused) { file_->nUnused_ = nUnused; }
void Stream::clearUnused() { file_->clearUnused(); }
} // namespace rosbag

114
thirdparty/ros/ros_comm/tools/rosbag_storage/src/uncompressed_stream.cpp vendored Normal file
View File

@@ -0,0 +1,114 @@
/*********************************************************************
* 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/chunked_file.h"
#include <iostream>
#include <cstring>
#include <boost/format.hpp>
using std::string;
using boost::format;
using ros::Exception;
namespace rosbag {
UncompressedStream::UncompressedStream(ChunkedFile* file) : Stream(file) { }
CompressionType UncompressedStream::getCompressionType() const {
return compression::Uncompressed;
}
void UncompressedStream::write(void* ptr, size_t size) {
size_t result = fwrite(ptr, 1, size, getFilePointer());
if (result != size)
throw BagIOException((format("Error writing to file: writing %1% bytes, wrote %2% bytes") % size % result).str());
advanceOffset(size);
}
void UncompressedStream::read(void* ptr, size_t size) {
size_t nUnused = (size_t) getUnusedLength();
char* unused = getUnused();
if (nUnused > 0) {
// We have unused data from the last compressed read
if (nUnused == size) {
// Copy the unused data into the buffer
memcpy(ptr, unused, nUnused);
clearUnused();
}
else if (nUnused < size) {
// Copy the unused data into the buffer
memcpy(ptr, unused, nUnused);
// Still have data to read
size -= nUnused;
// Read the remaining data from the file
int result = fread((char*) ptr + nUnused, 1, size, getFilePointer());
if ((size_t) result != size)
throw BagIOException((format("Error reading from file + unused: wanted %1% bytes, read %2% bytes") % size % result).str());
advanceOffset(size);
clearUnused();
}
else {
// nUnused_ > size
memcpy(ptr, unused, size);
setUnused(unused + size);
setUnusedLength(nUnused - size);
}
}
// No unused data - read from stream
int result = fread(ptr, 1, size, getFilePointer());
if ((size_t) result != size)
throw BagIOException((format("Error reading from file: wanted %1% bytes, read %2% bytes") % size % result).str());
advanceOffset(size);
}
void UncompressedStream::decompress(uint8_t* dest, unsigned int dest_len, uint8_t* source, unsigned int source_len) {
if (dest_len < source_len)
throw BagException("dest_len not large enough");
memcpy(dest, source, source_len);
}
} // namespace rosbag

350
thirdparty/ros/ros_comm/tools/rosbag_storage/src/view.cpp vendored Normal file
View File

@@ -0,0 +1,350 @@
// 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.
#include "rosbag/view.h"
#include "rosbag/bag.h"
#include "rosbag/message_instance.h"
#include <boost/foreach.hpp>
#include <set>
#include <assert.h>
#define foreach BOOST_FOREACH
using std::map;
using std::string;
using std::vector;
using std::multiset;
namespace rosbag {
// View::iterator
View::iterator::iterator() : view_(NULL), view_revision_(0), message_instance_(NULL) { }
View::iterator::~iterator()
{
if (message_instance_ != NULL)
delete message_instance_;
}
View::iterator::iterator(View* view, bool end) : view_(view), view_revision_(0), message_instance_(NULL) {
if (view != NULL && !end)
populate();
}
View::iterator::iterator(const iterator& i) : view_(i.view_), iters_(i.iters_), view_revision_(i.view_revision_), message_instance_(NULL) { }
View::iterator &View::iterator::operator=(iterator const& i) {
if (this != &i) {
view_ = i.view_;
iters_ = i.iters_;
view_revision_ = i.view_revision_;
if (message_instance_ != NULL) {
delete message_instance_;
message_instance_ = NULL;
}
}
return *this;
}
void View::iterator::populate() {
assert(view_ != NULL);
iters_.clear();
foreach(MessageRange const* range, view_->ranges_)
if (range->begin != range->end)
iters_.push_back(ViewIterHelper(range->begin, range));
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
view_revision_ = view_->view_revision_;
}
void View::iterator::populateSeek(multiset<IndexEntry>::const_iterator iter) {
assert(view_ != NULL);
iters_.clear();
foreach(MessageRange const* range, view_->ranges_) {
multiset<IndexEntry>::const_iterator start = std::lower_bound(range->begin, range->end, iter->time, IndexEntryCompare());
if (start != range->end)
iters_.push_back(ViewIterHelper(start, range));
}
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
while (iter != iters_.back().iter)
increment();
view_revision_ = view_->view_revision_;
}
bool View::iterator::equal(View::iterator const& other) const {
// We need some way of verifying these are actually talking about
// the same merge_queue data since we shouldn't be able to compare
// iterators from different Views.
if (iters_.empty())
return other.iters_.empty();
if (other.iters_.empty())
return false;
return iters_.back().iter == other.iters_.back().iter;
}
void View::iterator::increment() {
assert(view_ != NULL);
// Our message instance is no longer valid
if (message_instance_ != NULL)
{
delete message_instance_;
message_instance_ = NULL;
}
view_->update();
// Note, updating may have blown away our message-ranges and
// replaced them in general the ViewIterHelpers are no longer
// valid, but the iterator it stores should still be good.
if (view_revision_ != view_->view_revision_)
populateSeek(iters_.back().iter);
if (view_->reduce_overlap_)
{
std::multiset<IndexEntry>::const_iterator last_iter = iters_.back().iter;
while (!iters_.empty() && iters_.back().iter == last_iter)
{
iters_.back().iter++;
if (iters_.back().iter == iters_.back().range->end)
iters_.pop_back();
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
}
} else {
iters_.back().iter++;
if (iters_.back().iter == iters_.back().range->end)
iters_.pop_back();
std::sort(iters_.begin(), iters_.end(), ViewIterHelperCompare());
}
}
MessageInstance& View::iterator::dereference() const {
ViewIterHelper const& i = iters_.back();
if (message_instance_ == NULL)
message_instance_ = view_->newMessageInstance(i.range->connection_info, *(i.iter), *(i.range->bag_query->bag));
return *message_instance_;
}
// View
View::View(bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) { }
View::View(Bag const& bag, ros::Time const& start_time, ros::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
addQuery(bag, start_time, end_time);
}
View::View(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time, ros::Time const& end_time, bool const& reduce_overlap) : view_revision_(0), size_cache_(0), size_revision_(0), reduce_overlap_(reduce_overlap) {
addQuery(bag, query, start_time, end_time);
}
View::~View() {
foreach(MessageRange* range, ranges_)
delete range;
foreach(BagQuery* query, queries_)
delete query;
}
ros::Time View::getBeginTime()
{
update();
ros::Time begin = ros::TIME_MAX;
foreach (rosbag::MessageRange* range, ranges_)
{
if (range->begin->time < begin)
begin = range->begin->time;
}
return begin;
}
ros::Time View::getEndTime()
{
update();
ros::Time end = ros::TIME_MIN;
foreach (rosbag::MessageRange* range, ranges_)
{
std::multiset<IndexEntry>::const_iterator e = range->end;
e--;
if (e->time > end)
end = e->time;
}
return end;
}
//! Simply copy the merge_queue state into the iterator
View::iterator View::begin() {
update();
return iterator(this);
}
//! Default constructed iterator signifies end
View::iterator View::end() { return iterator(this, true); }
uint32_t View::size() {
update();
if (size_revision_ != view_revision_)
{
size_cache_ = 0;
foreach (MessageRange* range, ranges_)
{
size_cache_ += std::distance(range->begin, range->end);
}
size_revision_ = view_revision_;
}
return size_cache_;
}
void View::addQuery(Bag const& bag, ros::Time const& start_time, ros::Time const& end_time) {
if ((bag.getMode() & bagmode::Read) != bagmode::Read)
throw BagException("Bag not opened for reading");
boost::function<bool(ConnectionInfo const*)> query = TrueQuery();
queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
updateQueries(queries_.back());
}
void View::addQuery(Bag const& bag, boost::function<bool(ConnectionInfo const*)> query, ros::Time const& start_time, ros::Time const& end_time) {
if ((bag.getMode() & bagmode::Read) != bagmode::Read)
throw BagException("Bag not opened for reading");
queries_.push_back(new BagQuery(&bag, Query(query, start_time, end_time), bag.bag_revision_));
updateQueries(queries_.back());
}
void View::updateQueries(BagQuery* q) {
for (map<uint32_t, ConnectionInfo*>::const_iterator i = q->bag->connections_.begin(); i != q->bag->connections_.end(); i++) {
ConnectionInfo const* connection = i->second;
// Skip if the query doesn't evaluate to true
if (!q->query.getQuery()(connection))
continue;
map<uint32_t, multiset<IndexEntry> >::const_iterator j = q->bag->connection_indexes_.find(connection->id);
// Skip if the bag doesn't have the corresponding index
if (j == q->bag->connection_indexes_.end())
continue;
multiset<IndexEntry> const& index = j->second;
// lower_bound/upper_bound do a binary search to find the appropriate range of Index Entries given our time range
IndexEntry start_time_lookup_entry = { q->query.getStartTime(), 0, 0 };
IndexEntry end_time_lookup_entry = { q->query.getEndTime() , 0, 0 };
std::multiset<IndexEntry>::const_iterator begin = index.lower_bound(start_time_lookup_entry);
std::multiset<IndexEntry>::const_iterator end = index.upper_bound(end_time_lookup_entry);
// Make sure we are at the right beginning
while (begin != index.begin() && begin->time >= q->query.getStartTime())
{
begin--;
if (begin->time < q->query.getStartTime())
{
begin++;
break;
}
}
if (begin != end)
{
// todo: make faster with a map of maps
bool found = false;
for (vector<MessageRange*>::iterator k = ranges_.begin(); k != ranges_.end(); k++) {
MessageRange* r = *k;
// If the topic and query are already in our ranges, we update
if (r->bag_query == q && r->connection_info->id == connection->id) {
r->begin = begin;
r->end = end;
found = true;
break;
}
}
if (!found)
ranges_.push_back(new MessageRange(begin, end, connection, q));
}
}
view_revision_++;
}
void View::update() {
foreach(BagQuery* query, queries_) {
if (query->bag->bag_revision_ != query->bag_revision) {
updateQueries(query);
query->bag_revision = query->bag->bag_revision_;
}
}
}
std::vector<const ConnectionInfo*> View::getConnections()
{
std::vector<const ConnectionInfo*> connections;
foreach(MessageRange* range, ranges_)
{
connections.push_back(range->connection_info);
}
return connections;
}
MessageInstance* View::newMessageInstance(ConnectionInfo const* connection_info, IndexEntry const& index, Bag const& bag)
{
return new MessageInstance(connection_info, index, bag);
}
} // namespace rosbag

180
thirdparty/ros/ros_comm/tools/rosconsole/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,180 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rosconsole
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.12.14 (2018-08-23)
--------------------
1.12.13 (2018-02-21)
--------------------
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)
-------------------
* replace 'while(0)' with 'while(false)' to avoid warnings (`#1179 <https://github.com/ros/ros_comm/issues/1179>`_)
1.12.7 (2017-02-17)
-------------------
1.12.6 (2016-10-26)
-------------------
* add missing walltime to roscpp logging (`#879 <https://github.com/ros/ros_comm/pull/879>`_)
* fix building on GCC-6 (`#911 <https://github.com/ros/ros_comm/pull/911>`_)
1.12.5 (2016-09-30)
-------------------
1.12.4 (2016-09-19)
-------------------
1.12.3 (2016-09-17)
-------------------
1.12.2 (2016-06-03)
-------------------
1.12.1 (2016-04-18)
-------------------
* use directory specific compiler flags (`#785 <https://github.com/ros/ros_comm/pull/785>`_)
1.12.0 (2016-03-18)
-------------------
* make LogAppender and Token destructor virtual (`#729 <https://github.com/ros/ros_comm/issues/729>`_)
1.11.18 (2016-03-17)
--------------------
* fix compiler warnings
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)
--------------------
1.11.15 (2015-10-13)
--------------------
1.11.14 (2015-09-19)
--------------------
* avoid redefining ROS_ASSERT_ENABLED (`#628 <https://github.com/ros/ros_comm/pull/628>`_)
1.11.13 (2015-04-28)
--------------------
1.11.12 (2015-04-27)
--------------------
1.11.11 (2015-04-16)
--------------------
* add DELAYED_THROTTLE versions of log macros (`#571 <https://github.com/ros/ros_comm/issues/571>`_)
1.11.10 (2014-12-22)
--------------------
* fix various defects reported by coverity
1.11.9 (2014-08-18)
-------------------
1.11.8 (2014-08-04)
-------------------
1.11.7 (2014-07-18)
-------------------
1.11.6 (2014-07-10)
-------------------
1.11.5 (2014-06-24)
-------------------
* rename variables within rosconsole macros (`#442 <https://github.com/ros/ros_comm/issues/442>`_)
1.11.4 (2014-06-16)
-------------------
1.11.3 (2014-05-21)
-------------------
1.11.2 (2014-05-08)
-------------------
1.11.1 (2014-05-07)
-------------------
1.11.0 (2014-03-04)
-------------------
1.10.0 (2014-02-11)
-------------------
1.9.54 (2014-01-27)
-------------------
* fix rosconsole segfault when using ROSCONSOLE_FORMAT with (`#342 <https://github.com/ros/ros_comm/issues/342>`_)
* add missing run/test dependencies on rosbuild to get ROS_ROOT environment variable
1.9.53 (2014-01-14)
-------------------
* readd g_level_lockup symbol for backward compatibility when log4cxx is being used
1.9.52 (2014-01-08)
-------------------
* fix missing export of rosconsole backend interface library
1.9.51 (2014-01-07)
-------------------
* refactor rosconsole to not expose log4cxx, implement empty and log4cxx backends
1.9.50 (2013-10-04)
-------------------
1.9.49 (2013-09-16)
-------------------
1.9.48 (2013-08-21)
-------------------
* wrap condition in ROS_ASSERT_CMD in parenthesis (`#271 <https://github.com/ros/ros_comm/issues/271>`_)
1.9.47 (2013-07-03)
-------------------
* force CMake policy before setting preprocessor definition to ensure correct escaping (`#245 <https://github.com/ros/ros_comm/issues/245>`_)
* check for CATKIN_ENABLE_TESTING to enable configure without tests
1.9.46 (2013-06-18)
-------------------
1.9.45 (2013-06-06)
-------------------
1.9.44 (2013-03-21)
-------------------
* fix install destination for dll's under Windows
1.9.43 (2013-03-13)
-------------------
1.9.42 (2013-03-08)
-------------------
* fix handling spaces in folder names (`ros/catkin#375 <https://github.com/ros/catkin/issues/375>`_)
1.9.41 (2013-01-24)
-------------------
1.9.40 (2013-01-13)
-------------------
* fix dependent packages by pass LOG4CXX include dirs and libraries along
* fix usage of variable arguments in vFormatToBuffer() function
1.9.39 (2012-12-29)
-------------------
* first public release for Groovy

127
thirdparty/ros/ros_comm/tools/rosconsole/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,127 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosconsole)
if(NOT WIN32)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-Wall;-Wextra")
endif()
find_package(catkin REQUIRED COMPONENTS cpp_common rostime rosunit)
find_package(Boost COMPONENTS regex system thread)
# select rosconsole backend
set(ROSCONSOLE_BACKEND "" CACHE STRING "Type of rosconsole backend, one of 'log4cxx', 'glog', 'print'")
set(rosconsole_backend_INCLUDE_DIRS)
set(rosconsole_backend_LIBRARIES)
if(ROSCONSOLE_BACKEND STREQUAL "" OR ROSCONSOLE_BACKEND STREQUAL "log4cxx")
find_package(Log4cxx QUIET)
if(NOT LOG4CXX_LIBRARIES)
# backup plan, hope it is in the system path
find_library(LOG4CXX_LIBRARIES log4cxx)
endif()
if(LOG4CXX_LIBRARIES)
list(APPEND rosconsole_backend_INCLUDE_DIRS ${LOG4CXX_INCLUDE_DIRS})
list(APPEND rosconsole_backend_LIBRARIES rosconsole_log4cxx rosconsole_backend_interface ${LOG4CXX_LIBRARIES})
set(ROSCONSOLE_BACKEND "log4cxx")
elseif(ROSCONSOLE_BACKEND STREQUAL "log4cxx")
message(FATAL_ERROR "Couldn't find log4cxx library")
endif()
endif()
if(ROSCONSOLE_BACKEND STREQUAL "" OR ROSCONSOLE_BACKEND STREQUAL "glog")
find_package(PkgConfig)
pkg_check_modules(GLOG QUIET libglog)
if(GLOG_FOUND)
list(APPEND rosconsole_backend_INCLUDE_DIRS ${GLOG_INCLUDE_DIRS})
list(APPEND rosconsole_backend_LIBRARIES rosconsole_glog rosconsole_backend_interface ${GLOG_LIBRARIES})
set(ROSCONSOLE_BACKEND "glog")
elseif(ROSCONSOLE_BACKEND STREQUAL "glog")
message(FATAL_ERROR "Couldn't find glog library")
endif()
endif()
if(ROSCONSOLE_BACKEND STREQUAL "" OR ROSCONSOLE_BACKEND STREQUAL "print")
list(APPEND rosconsole_backend_LIBRARIES rosconsole_print rosconsole_backend_interface)
set(ROSCONSOLE_BACKEND "print")
endif()
message(STATUS "rosconsole backend: ${ROSCONSOLE_BACKEND}")
catkin_package(
INCLUDE_DIRS include ${rosconsole_backend_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}
LIBRARIES rosconsole ${rosconsole_backend_LIBRARIES} ${Boost_LIBRARIES}
CATKIN_DEPENDS cpp_common rostime
CFG_EXTRAS rosconsole-extras.cmake
)
include(${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/cmake/rosconsole-extras.cmake)
# See ticket: https://code.ros.org/trac/ros/ticket/3626
# On mac use g++-4.2
IF(${CMAKE_SYSTEM} MATCHES "Darwin-11.*")
IF(EXISTS "/usr/bin/g++-4.2")
set(CMAKE_CXX_COMPILER /usr/bin/g++-4.2)
ELSE(EXISTS "/usr/bin/g++-4.2")
# If there is no g++-4.2 use clang++
set(CMAKE_CXX_COMPILER /usr/bin/clang++)
ENDIF(EXISTS "/usr/bin/g++-4.2")
ENDIF(${CMAKE_SYSTEM} MATCHES "Darwin-11.*")
include_directories(include ${catkin_INCLUDE_DIRS} ${rosconsole_backend_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
add_library(rosconsole_backend_interface src/rosconsole/rosconsole_backend.cpp)
add_library(rosconsole src/rosconsole/rosconsole.cpp)
target_link_libraries(rosconsole ${rosconsole_backend_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
if(ROSCONSOLE_BACKEND STREQUAL "log4cxx")
add_library(rosconsole_log4cxx src/rosconsole/impl/rosconsole_log4cxx.cpp)
target_link_libraries(rosconsole_log4cxx rosconsole_backend_interface ${LOG4CXX_LIBRARIES} ${Boost_LIBRARIES})
if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/test/speed_test.cpp")
add_executable(rosconsole_speed_test test/speed_test.cpp)
target_link_libraries(rosconsole_speed_test rosconsole ${rosconsole_backend_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES})
endif()
elseif(ROSCONSOLE_BACKEND STREQUAL "glog")
add_library(rosconsole_glog src/rosconsole/impl/rosconsole_glog.cpp)
target_link_libraries(rosconsole_glog rosconsole_backend_interface ${GLOG_LIBRARIES})
elseif(ROSCONSOLE_BACKEND STREQUAL "print")
add_library(rosconsole_print src/rosconsole/impl/rosconsole_print.cpp)
target_link_libraries(rosconsole_print rosconsole_backend_interface)
else()
message(FATAL_ERROR "Unknown rosconsole backend '${ROSCONSOLE_BACKEND}'")
endif()
if(CMAKE_HOST_UNIX)
catkin_add_env_hooks(10.rosconsole SHELLS sh DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
else()
catkin_add_env_hooks(10.rosconsole SHELLS bat DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/env-hooks)
endif()
install(TARGETS rosconsole rosconsole_${ROSCONSOLE_BACKEND} rosconsole_backend_interface
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(FILES config/rosconsole.config
DESTINATION ${CATKIN_GLOBAL_SHARE_DESTINATION}/ros/config)
install(DIRECTORY include/
DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h")
if(CATKIN_ENABLE_TESTING)
catkin_add_gtest(${PROJECT_NAME}-utest test/utest.cpp)
if(TARGET ${PROJECT_NAME}-utest)
target_link_libraries(${PROJECT_NAME}-utest ${PROJECT_NAME})
endif()
if(${CMAKE_SYSTEM_NAME} STREQUAL Linux)
catkin_add_gtest(${PROJECT_NAME}-assertion_test test/assertion_test.cpp)
if(TARGET ${PROJECT_NAME}-assertion_test)
target_link_libraries(${PROJECT_NAME}-assertion_test ${PROJECT_NAME})
endif()
endif()
catkin_add_gtest(${PROJECT_NAME}-thread_test test/thread_test.cpp)
if(TARGET ${PROJECT_NAME}-thread_test)
target_link_libraries(${PROJECT_NAME}-thread_test ${PROJECT_NAME})
endif()
endif()

15
thirdparty/ros/ros_comm/tools/rosconsole/cmake/rosconsole-extras.cmake.in vendored Normal file
View File

@@ -0,0 +1,15 @@
# ros_comm/tools/rosconsole/cmake/rosconsole-extras.cmake
# force automatic escaping of preprocessor definitions
cmake_policy(PUSH)
cmake_policy(SET CMP0005 NEW)
# add ROS_PACKAGE_NAME define required by the named logging macros
add_definitions(-DROS_PACKAGE_NAME=\"${PROJECT_NAME}\")
if("@ROSCONSOLE_BACKEND@" STREQUAL "log4cxx")
# add ROSCONSOLE_BACKEND_LOG4CXX define required for backward compatible log4cxx symbols
add_definitions(-DROSCONSOLE_BACKEND_LOG4CXX)
endif()
cmake_policy(POP)

8
thirdparty/ros/ros_comm/tools/rosconsole/config/rosconsole.config vendored Normal file
View File

@@ -0,0 +1,8 @@
#
# rosconsole will find this file by default at $ROS_ROOT/config/rosconsole.config
#
# You can define your own by e.g. copying this file and setting
# ROSCONSOLE_CONFIG_FILE (in your environment) to point to the new file
#
log4j.logger.ros=INFO
log4j.logger.ros.roscpp.superdebug=WARN

3
thirdparty/ros/ros_comm/tools/rosconsole/env-hooks/10.rosconsole.bat.develspace.em vendored Normal file
View File

@@ -0,0 +1,3 @@
REM generated from rosconsole/env-hooks/10.rosconsole.bat.develspace.em
set ROSCONSOLE_CONFIG_FILE=@(CMAKE_CURRENT_SOURCE_DIR)/config/rosconsole.config

3
thirdparty/ros/ros_comm/tools/rosconsole/env-hooks/10.rosconsole.sh.develspace.em vendored Normal file
View File

@@ -0,0 +1,3 @@
# generated from rosconsole/env-hooks/10.rosconsole.sh.develspace.em
export ROSCONSOLE_CONFIG_FILE="@(CMAKE_CURRENT_SOURCE_DIR)/config/rosconsole.config"

86
thirdparty/ros/ros_comm/tools/rosconsole/examples/example.cpp vendored Normal file
View File

@@ -0,0 +1,86 @@
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/
#include "ros/console.h"
#include <log4cxx/logger.h>
void print(ros::console::Level level, const std::string& s)
{
ROS_LOG(level, ROSCONSOLE_DEFAULT_NAME, "%s", s.c_str());
}
int main(int /*argc*/, char** /*argv*/)
{
// This needs to happen before we start fooling around with logger levels. Otherwise the level we set may be overwritten by
// a configuration file
ROSCONSOLE_AUTOINIT;
log4cxx::LoggerPtr my_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
// Set the logger for this package to output all statements
my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
// All these should print
ROS_DEBUG("This is a debug statement, and should print");
ROS_INFO("This is an info statement, and should print");
ROS_WARN("This is a warn statement, and should print");
ROS_ERROR("This is an error statement, and should print");
ROS_FATAL("This is a fatal statement, and should print");
// This should also print
print(ros::console::levels::Debug, "Hello, this should print");
my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Error]);
// This will STILL print, because the logger's enabled state has been cached
print(ros::console::levels::Debug, "Hello, this will also print");
// Calling notifyLoggerLevelsChanged() will force a reevaluation of which logging statements are enabled
ros::console::notifyLoggerLevelsChanged();
// Which will cause this to not print
print(ros::console::levels::Debug, "Hello, this will NOT print");
log4cxx::LoggerPtr ros_logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
my_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
ros_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Error]);
ROS_DEBUG("This will still print, because the child logger's level overrides its ancestor loggers' levels");
log4cxx::LoggerPtr test_logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME".test");
test_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Error]);
ROS_INFO_NAMED("test", "This will not print, because the ros.rosconsole.test logger has been set to Error verbosity");
test_logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Debug]);
ROS_INFO_NAMED("test", "Now everything sent to the ros.rosconsole.test logger will be printed (including this)");
return 0;
}

155
thirdparty/ros/ros_comm/tools/rosconsole/include/ros/assert.h vendored Normal file
View File

@@ -0,0 +1,155 @@
/*
* 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.
*/
// Author: Josh Faust
#ifndef ROSCONSOLE_ROSASSERT_H
#define ROSCONSOLE_ROSASSERT_H
#include "ros/console.h"
#include "ros/static_assert.h"
/** \file */
/** \def ROS_ASSERT(cond)
* \brief Asserts that the provided condition evaluates to true.
*
* If it is false, program execution will abort, with an informative
* statement about which assertion failed, in what file. Use ROS_ASSERT
* instead of assert() itself.
*
* If running inside a debugger, ROS_ASSERT will allow you to step past the assertion.
*/
/** \def ROS_ASSERT_MSG(cond, ...)
* \brief Asserts that the provided condition evaluates to true.
*
* If it is false, program execution will abort, with an informative
* statement about which assertion failed, in what file, and it will print out
* a printf-style message you define. Example usage:
@verbatim
ROS_ASSERT_MSG(x > 0, "Uh oh, x went negative. Value = %d", x);
@endverbatim
*
* If running inside a debugger, ROS_ASSERT will allow you to step past the assertion.
*/
/**
* \def ROS_ASSERT_CMD()
* \brief Runs a command if the provided condition is false
*
* For example:
\verbatim
ROS_ASSERT_CMD(x > 0, handleError(...));
\endverbatim
*/
/** \def ROS_BREAK()
* \brief Aborts program execution.
*
* Aborts program execution with an informative message stating what file and
* line it was called from. Use ROS_BREAK instead of calling assert(0) or
* ROS_ASSERT(0).
*
* If running inside a debugger, ROS_BREAK will allow you to step past the breakpoint.
*/
/** \def ROS_ISSUE_BREAK()
* \brief Always issues a breakpoint instruction.
*
* This define is mostly for internal use, but is useful if you want to simply issue a break
* instruction in a cross-platform way.
*
* Currently implemented for Windows (any platform), powerpc64, and x86
*/
#include <ros/platform.h>
#ifdef WIN32
# if defined (__MINGW32__)
# define ROS_ISSUE_BREAK() DebugBreak();
# else // MSVC
# define ROS_ISSUE_BREAK() __debugbreak();
# endif
#elif defined(__powerpc64__)
# define ROS_ISSUE_BREAK() asm volatile ("tw 31,1,1");
#elif defined(__i386__) || defined(__ia64__) || defined(__x86_64__)
# define ROS_ISSUE_BREAK() asm("int $3");
#else
# include <stdlib.h>
# define ROS_ISSUE_BREAK() abort();
#endif
#ifndef NDEBUG
#ifndef ROS_ASSERT_ENABLED
#define ROS_ASSERT_ENABLED
#endif
#endif
#ifdef ROS_ASSERT_ENABLED
#define ROS_BREAK() \
do { \
ROS_FATAL("BREAKPOINT HIT\n\tfile = %s\n\tline=%d\n", __FILE__, __LINE__); \
ROS_ISSUE_BREAK() \
} while (false)
#define ROS_ASSERT(cond) \
do { \
if (!(cond)) { \
ROS_FATAL("ASSERTION FAILED\n\tfile = %s\n\tline = %d\n\tcond = %s\n", __FILE__, __LINE__, #cond); \
ROS_ISSUE_BREAK() \
} \
} while (false)
#define ROS_ASSERT_MSG(cond, ...) \
do { \
if (!(cond)) { \
ROS_FATAL("ASSERTION FAILED\n\tfile = %s\n\tline = %d\n\tcond = %s\n\tmessage = ", __FILE__, __LINE__, #cond); \
ROS_FATAL(__VA_ARGS__); \
ROS_FATAL("\n"); \
ROS_ISSUE_BREAK(); \
} \
} while (false)
#define ROS_ASSERT_CMD(cond, cmd) \
do { \
if (!(cond)) { \
cmd; \
} \
} while (false)
#else
#define ROS_BREAK()
#define ROS_ASSERT(cond)
#define ROS_ASSERT_MSG(cond, ...)
#define ROS_ASSERT_CMD(cond, cmd)
#endif
#endif // ROSCONSOLE_ROSASSERT_H

572
thirdparty/ros/ros_comm/tools/rosconsole/include/ros/console.h vendored Normal file
View File

@@ -0,0 +1,572 @@
/*
* 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.
*/
// Author: Josh Faust
#ifndef ROSCONSOLE_ROSCONSOLE_H
#define ROSCONSOLE_ROSCONSOLE_H
#include "console_backend.h"
#include <cstdio>
#include <sstream>
#include <ros/time.h>
#include <cstdarg>
#include <ros/macros.h>
#include <map>
#include <vector>
#ifdef ROSCONSOLE_BACKEND_LOG4CXX
#include "log4cxx/level.h"
#endif
// Import/export for windows dll's and visibility for gcc shared libraries.
#ifdef ROS_BUILD_SHARED_LIBS // ros is being built around shared libraries
#ifdef rosconsole_EXPORTS // we are building a shared lib/dll
#define ROSCONSOLE_DECL ROS_HELPER_EXPORT
#else // we are using shared lib/dll
#define ROSCONSOLE_DECL ROS_HELPER_IMPORT
#endif
#else // ros is being built around static libraries
#define ROSCONSOLE_DECL
#endif
#ifdef __GNUC__
#if __GNUC__ >= 3
#define ROSCONSOLE_PRINTF_ATTRIBUTE(a, b) __attribute__ ((__format__ (__printf__, a, b)));
#endif
#endif
#ifndef ROSCONSOLE_PRINTF_ATTRIBUTE
#define ROSCONSOLE_PRINTF_ATTRIBUTE(a, b)
#endif
namespace boost
{
template<typename T> class shared_array;
}
namespace ros
{
namespace console
{
ROSCONSOLE_DECL void shutdown();
#ifdef ROSCONSOLE_BACKEND_LOG4CXX
extern ROSCONSOLE_DECL log4cxx::LevelPtr g_level_lookup[];
#endif
extern ROSCONSOLE_DECL bool get_loggers(std::map<std::string, levels::Level>& loggers);
extern ROSCONSOLE_DECL bool set_logger_level(const std::string& name, levels::Level level);
/**
* \brief Only exported because the macros need it. Do not use directly.
*/
extern ROSCONSOLE_DECL bool g_initialized;
/**
* \brief Only exported because the TopicManager need it. Do not use directly.
*/
extern ROSCONSOLE_DECL std::string g_last_error_message;
class LogAppender
{
public:
virtual ~LogAppender() {}
virtual void log(::ros::console::Level level, const char* str, const char* file, const char* function, int line) = 0;
};
ROSCONSOLE_DECL void register_appender(LogAppender* appender);
struct Token
{
virtual ~Token() {}
/*
* @param level
* @param message
* @param file
* @param function
* @param line
*/
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int) = 0;
};
typedef boost::shared_ptr<Token> TokenPtr;
typedef std::vector<TokenPtr> V_Token;
struct Formatter
{
void init(const char* fmt);
void print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line);
std::string format_;
V_Token tokens_;
};
/**
* \brief Only exported because the implementation need it. Do not use directly.
*/
extern ROSCONSOLE_DECL Formatter g_formatter;
/**
* \brief Don't call this directly. Performs any required initialization/configuration. Happens automatically when using the macro API.
*
* If you're going to be using log4cxx or any of the ::ros::console functions, and need the system to be initialized, use the
* ROSCONSOLE_AUTOINIT macro.
*/
ROSCONSOLE_DECL void initialize();
class FilterBase;
/**
* \brief Don't call this directly. Use the ROS_LOG() macro instead.
* @param level Logging level
* @param file File this logging statement is from (usually generated with __FILE__)
* @param line Line of code this logging statement is from (usually generated with __LINE__)
* @param fmt Format string
*/
ROSCONSOLE_DECL void print(FilterBase* filter, void* logger, Level level,
const char* file, int line,
const char* function, const char* fmt, ...) ROSCONSOLE_PRINTF_ATTRIBUTE(7, 8);
ROSCONSOLE_DECL void print(FilterBase* filter, void* logger, Level level,
const std::stringstream& str, const char* file, int line, const char* function);
struct ROSCONSOLE_DECL LogLocation;
/**
* \brief Registers a logging location with the system.
*
* This is used for the case where a logger's verbosity level changes, and we need to reset the enabled status of
* all the logging statements.
* @param loc The location to add
*/
ROSCONSOLE_DECL void registerLogLocation(LogLocation* loc);
/**
* \brief Tells the system that a logger's level has changed
*
* This must be called if a log4cxx::Logger's level has been changed in the middle of an application run.
* Because of the way the static guard for enablement works, if a logger's level is changed and this
* function is not called, only logging statements which are first hit *after* the change will be correct wrt
* that logger.
*/
ROSCONSOLE_DECL void notifyLoggerLevelsChanged();
ROSCONSOLE_DECL void setFixedFilterToken(const std::string& key, const std::string& val);
/**
* \brief Parameter structure passed to FilterBase::isEnabled(...);. Includes both input and output parameters
*/
struct FilterParams
{
// input parameters
const char* file; ///< [input] File the message came from
int line; ///< [input] Line the message came from
const char* function; ///< [input] Function the message came from
const char* message; ///< [input] The formatted message that will be output
// input/output parameters
void* logger; ///< [input/output] Handle identifying logger that this message will be output to. If changed, uses the new logger
Level level; ///< [input/output] Severity level. If changed, uses the new level
// output parameters
std::string out_message; ///< [output] If set, writes this message instead of the original
};
/**
* \brief Base-class for filters. Filters allow full user-defined control over whether or not a message should print.
* The ROS_X_FILTER... macros provide the filtering functionality.
*
* Filters get a chance to veto the message from printing at two times: first before the message arguments are
* evaluated and the message is formatted, and then once the message is formatted before it is printed. It is also possible
* to change the message, logger and severity level at this stage (see the FilterParams struct for more details).
*
* When a ROS_X_FILTER... macro is called, here is the high-level view of how it uses the filter passed in:
\verbatim
if (<logging level is enabled> && filter->isEnabled())
{
<format message>
<fill out FilterParams>
if (filter->isEnabled(params))
{
<print message>
}
}
\endverbatim
*/
class FilterBase
{
public:
virtual ~FilterBase() {}
/**
* \brief Returns whether or not the log statement should be printed. Called before the log arguments are evaluated
* and the message is formatted.
*/
inline virtual bool isEnabled() { return true; }
/**
* \brief Returns whether or not the log statement should be printed. Called once the message has been formatted,
* and allows you to change the message, logger and severity level if necessary.
*/
inline virtual bool isEnabled(FilterParams&) { return true; }
};
struct ROSCONSOLE_DECL LogLocation;
/**
* \brief Internal
*/
ROSCONSOLE_DECL void initializeLogLocation(LogLocation* loc, const std::string& name, Level level);
/**
* \brief Internal
*/
ROSCONSOLE_DECL void setLogLocationLevel(LogLocation* loc, Level level);
/**
* \brief Internal
*/
ROSCONSOLE_DECL void checkLogLocationEnabled(LogLocation* loc);
/**
* \brief Internal
*/
struct LogLocation
{
bool initialized_;
bool logger_enabled_;
::ros::console::Level level_;
void* logger_;
};
ROSCONSOLE_DECL void vformatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, va_list args);
ROSCONSOLE_DECL void formatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, ...);
ROSCONSOLE_DECL std::string formatToString(const char* fmt, ...);
} // namespace console
} // namespace ros
#ifdef WIN32
#define ROS_LIKELY(x) (x)
#define ROS_UNLIKELY(x) (x)
#else
#define ROS_LIKELY(x) __builtin_expect((x),1)
#define ROS_UNLIKELY(x) __builtin_expect((x),0)
#endif
#if defined(MSVC)
#define __ROSCONSOLE_FUNCTION__ __FUNCSIG__
#elif defined(__GNUC__)
#define __ROSCONSOLE_FUNCTION__ __PRETTY_FUNCTION__
#else
#define __ROSCONSOLE_FUNCTION__ ""
#endif
#ifdef ROS_PACKAGE_NAME
#define ROSCONSOLE_PACKAGE_NAME ROS_PACKAGE_NAME
#else
#define ROSCONSOLE_PACKAGE_NAME "unknown_package"
#endif
#define ROSCONSOLE_ROOT_LOGGER_NAME "ros"
#define ROSCONSOLE_NAME_PREFIX ROSCONSOLE_ROOT_LOGGER_NAME "." ROSCONSOLE_PACKAGE_NAME
#define ROSCONSOLE_DEFAULT_NAME ROSCONSOLE_NAME_PREFIX
// These allow you to compile-out everything below a certain severity level if necessary
#define ROSCONSOLE_SEVERITY_DEBUG 0
#define ROSCONSOLE_SEVERITY_INFO 1
#define ROSCONSOLE_SEVERITY_WARN 2
#define ROSCONSOLE_SEVERITY_ERROR 3
#define ROSCONSOLE_SEVERITY_FATAL 4
#define ROSCONSOLE_SEVERITY_NONE 5
/**
* \def ROSCONSOLE_MIN_SEVERITY
*
* Define ROSCONSOLE_MIN_SEVERITY=ROSCONSOLE_SEVERITY_[DEBUG|INFO|WARN|ERROR|FATAL] in your build options to compile out anything below that severity
*/
#ifndef ROSCONSOLE_MIN_SEVERITY
#define ROSCONSOLE_MIN_SEVERITY ROSCONSOLE_SEVERITY_DEBUG
#endif
/**
* \def ROSCONSOLE_AUTOINIT
* \brief Initializes the rosconsole library. Usually unnecessary to call directly.
*/
#define ROSCONSOLE_AUTOINIT \
do \
{ \
if (ROS_UNLIKELY(!::ros::console::g_initialized)) \
{ \
::ros::console::initialize(); \
} \
} while(false)
#define ROSCONSOLE_DEFINE_LOCATION(cond, level, name) \
ROSCONSOLE_AUTOINIT; \
static ::ros::console::LogLocation __rosconsole_define_location__loc = {false, false, ::ros::console::levels::Count, 0}; /* Initialized at compile-time */ \
if (ROS_UNLIKELY(!__rosconsole_define_location__loc.initialized_)) \
{ \
initializeLogLocation(&__rosconsole_define_location__loc, name, level); \
} \
if (ROS_UNLIKELY(__rosconsole_define_location__loc.level_ != level)) \
{ \
setLogLocationLevel(&__rosconsole_define_location__loc, level); \
checkLogLocationEnabled(&__rosconsole_define_location__loc); \
} \
bool __rosconsole_define_location__enabled = __rosconsole_define_location__loc.logger_enabled_ && (cond);
#define ROSCONSOLE_PRINT_AT_LOCATION_WITH_FILTER(filter, ...) \
::ros::console::print(filter, __rosconsole_define_location__loc.logger_, __rosconsole_define_location__loc.level_, __FILE__, __LINE__, __ROSCONSOLE_FUNCTION__, __VA_ARGS__)
#define ROSCONSOLE_PRINT_AT_LOCATION(...) \
ROSCONSOLE_PRINT_AT_LOCATION_WITH_FILTER(0, __VA_ARGS__)
// inside a macro which uses args use only well namespaced variable names in order to not overlay variables coming in via args
#define ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER(filter, args) \
do \
{ \
std::stringstream __rosconsole_print_stream_at_location_with_filter__ss__; \
__rosconsole_print_stream_at_location_with_filter__ss__ << args; \
::ros::console::print(filter, __rosconsole_define_location__loc.logger_, __rosconsole_define_location__loc.level_, __rosconsole_print_stream_at_location_with_filter__ss__, __FILE__, __LINE__, __ROSCONSOLE_FUNCTION__); \
} while (0)
#define ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args) \
ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER(0, args)
/**
* \brief Log to a given named logger at a given verbosity level, only if a given condition has been met, with printf-style formatting
*
* \note The condition will only be evaluated if this logging statement is enabled
*
* \param cond Boolean condition to be evaluated
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_COND(cond, level, name, ...) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(cond, level, name); \
\
if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) \
{ \
ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, only if a given condition has been met, with stream-style formatting
*
* \note The condition will only be evaluated if this logging statement is enabled
*
* \param cond Boolean condition to be evaluated
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_STREAM_COND(cond, level, name, args) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(cond, level, name); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled)) \
{ \
ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, only the first time it is hit when enabled, with printf-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_ONCE(level, name, ...) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
static bool hit = false; \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && ROS_UNLIKELY(!hit)) \
{ \
hit = true; \
ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
} \
} while(false)
// inside a macro which uses args use only well namespaced variable names in order to not overlay variables coming in via args
/**
* \brief Log to a given named logger at a given verbosity level, only the first time it is hit when enabled, with printf-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_STREAM_ONCE(level, name, args) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
static bool __ros_log_stream_once__hit__ = false; \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && ROS_UNLIKELY(!__ros_log_stream_once__hit__)) \
{ \
__ros_log_stream_once__hit__ = true; \
ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, limited to a specific rate of printing, with printf-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
* \param rate The rate it should actually trigger at
*/
#define ROS_LOG_THROTTLE(rate, level, name, ...) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
static double last_hit = 0.0; \
::ros::Time now = ::ros::Time::now(); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && ROS_UNLIKELY(last_hit + rate <= now.toSec())) \
{ \
last_hit = now.toSec(); \
ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
} \
} while(false)
// inside a macro which uses args use only well namespaced variable names in order to not overlay variables coming in via args
/**
* \brief Log to a given named logger at a given verbosity level, limited to a specific rate of printing, with printf-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
* \param rate The rate it should actually trigger at
*/
#define ROS_LOG_STREAM_THROTTLE(rate, level, name, args) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
static double __ros_log_stream_throttle__last_hit__ = 0.0; \
::ros::Time __ros_log_stream_throttle__now__ = ::ros::Time::now(); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && ROS_UNLIKELY(__ros_log_stream_throttle__last_hit__ + rate <= __ros_log_stream_throttle__now__.toSec())) \
{ \
__ros_log_stream_throttle__last_hit__ = __ros_log_stream_throttle__now__.toSec(); \
ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, limited to a specific rate of printing, with printf-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
* \param rate The rate it should actually trigger at
*/
#define ROS_LOG_DELAYED_THROTTLE(rate, level, name, ...) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
::ros::Time __ros_log_delayed_throttle__now__ = ::ros::Time::now(); \
static double __ros_log_delayed_throttle__last_hit__ = __ros_log_delayed_throttle__now__.toSec(); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && ROS_UNLIKELY(__ros_log_delayed_throttle__last_hit__ + rate <= __ros_log_delayed_throttle__now__.toSec())) \
{ \
__ros_log_delayed_throttle__last_hit__ = __ros_log_delayed_throttle__now__.toSec(); \
ROSCONSOLE_PRINT_AT_LOCATION(__VA_ARGS__); \
} \
} while(false)
// inside a macro which uses args use only well namespaced variable names in order to not overlay variables coming in via args
/**
* \brief Log to a given named logger at a given verbosity level, limited to a specific rate of printing and postponed first message
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
* \param rate The rate it should actually trigger at, and the delay before which no message will be shown.
*/
#define ROS_LOG_STREAM_DELAYED_THROTTLE(rate, level, name, args) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
::ros::Time __ros_log_stream_delayed_throttle__now__ = ::ros::Time::now(); \
static double __ros_log_stream_delayed_throttle__last_hit__ = __ros_log_stream_delayed_throttle__now__.toSec(); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && ROS_UNLIKELY(__ros_log_stream_delayed_throttle__last_hit__ + rate <= __ros_log_stream_delayed_throttle__now__.toSec())) \
{ \
__ros_log_stream_delayed_throttle__last_hit__ = __ros_log_stream_delayed_throttle__now__.toSec(); \
ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, with user-defined filtering, with printf-style formatting
*
* \param filter pointer to the filter to be used
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_FILTER(filter, level, name, ...) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && (filter)->isEnabled()) \
{ \
ROSCONSOLE_PRINT_AT_LOCATION_WITH_FILTER(filter, __VA_ARGS__); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, with user-defined filtering, with stream-style formatting
*
* \param cond Boolean condition to be evaluated
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_STREAM_FILTER(filter, level, name, args) \
do \
{ \
ROSCONSOLE_DEFINE_LOCATION(true, level, name); \
if (ROS_UNLIKELY(__rosconsole_define_location__enabled) && (filter)->isEnabled()) \
{ \
ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER(filter, args); \
} \
} while(false)
/**
* \brief Log to a given named logger at a given verbosity level, with printf-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG(level, name, ...) ROS_LOG_COND(true, level, name, __VA_ARGS__)
/**
* \brief Log to a given named logger at a given verbosity level, with stream-style formatting
*
* \param level One of the levels specified in ::ros::console::levels::Level
* \param name Name of the logger. Note that this is the fully qualified name, and does NOT include "ros.<package_name>". Use ROSCONSOLE_DEFAULT_NAME if you would like to use the default name.
*/
#define ROS_LOG_STREAM(level, name, args) ROS_LOG_STREAM_COND(true, level, name, args)
#include "rosconsole/macros_generated.h"
#endif // ROSCONSOLE_ROSCONSOLE_H

68
thirdparty/ros/ros_comm/tools/rosconsole/include/ros/console_backend.h vendored Normal file
View File

@@ -0,0 +1,68 @@
/*
* Copyright (c) 2013, Open Source Robotics Foundation
* 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 the 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 ROSCONSOLE_CONSOLE_BACKEND_H
#define ROSCONSOLE_CONSOLE_BACKEND_H
namespace ros
{
namespace console
{
namespace levels
{
enum Level
{
Debug,
Info,
Warn,
Error,
Fatal,
Count
};
}
typedef levels::Level Level;
namespace backend
{
void notifyLoggerLevelsChanged();
extern void (*function_notifyLoggerLevelsChanged)();
void print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line);
extern void (*function_print)(void*, ::ros::console::Level, const char*, const char*, const char*, int);
} // namespace backend
} // namespace console
} // namespace ros
#endif // ROSCONSOLE_CONSOLE_BACKEND_H

70
thirdparty/ros/ros_comm/tools/rosconsole/include/ros/static_assert.h vendored Normal file
View File

@@ -0,0 +1,70 @@
/*
* 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.
*/
// Author: Josh Faust
#ifndef ROSCONSOLE_STATIC_ASSERT_H
#define ROSCONSOLE_STATIC_ASSERT_H
// boost's static assert provides better errors messages in the failure case when using
// in templated situations
#include <boost/static_assert.hpp>
/**
* \def ROS_COMPILE_ASSERT(cond)
* \brief Compile-time assert.
*
* Only works with compile time statements, ie:
@verbatim
struct A
{
uint32_t a;
};
ROS_COMPILE_ASSERT(sizeof(A) == 4);
@endverbatim
*/
#define ROS_COMPILE_ASSERT(cond) BOOST_STATIC_ASSERT(cond)
/**
* \def ROS_STATIC_ASSERT(cond)
* \brief Compile-time assert.
*
* Only works with compile time statements, ie:
@verbatim
struct A
{
uint32_t a;
};
ROS_STATIC_ASSERT(sizeof(A) == 4);
@endverbatim
*/
#define ROS_STATIC_ASSERT(cond) BOOST_STATIC_ASSERT(cond)
#endif // ROSCONSOLE_STATIC_ASSERT_H

291
thirdparty/ros/ros_comm/tools/rosconsole/include/rosconsole/macros_generated.h vendored Normal file
View File

@@ -0,0 +1,291 @@
// !!!!!!!!!!!!!!!!!!!!!!! This is a generated file, do not edit manually
/*
* 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.
*/
#if (ROSCONSOLE_MIN_SEVERITY > ROSCONSOLE_SEVERITY_DEBUG)
#define ROS_DEBUG(...)
#define ROS_DEBUG_STREAM(args)
#define ROS_DEBUG_NAMED(name, ...)
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_DEBUG_COND(cond, ...)
#define ROS_DEBUG_STREAM_COND(cond, args)
#define ROS_DEBUG_COND_NAMED(cond, name, ...)
#define ROS_DEBUG_STREAM_COND_NAMED(cond, name, args)
#define ROS_DEBUG_ONCE(...)
#define ROS_DEBUG_STREAM_ONCE(args)
#define ROS_DEBUG_ONCE_NAMED(name, ...)
#define ROS_DEBUG_STREAM_ONCE_NAMED(name, args)
#define ROS_DEBUG_THROTTLE(rate, ...)
#define ROS_DEBUG_STREAM_THROTTLE(rate, args)
#define ROS_DEBUG_THROTTLE_NAMED(rate, name, ...)
#define ROS_DEBUG_STREAM_THROTTLE_NAMED(rate, name, args)
#define ROS_DEBUG_DELAYED_THROTTLE(rate, ...)
#define ROS_DEBUG_STREAM_DELAYED_THROTTLE(rate, args)
#define ROS_DEBUG_DELAYED_THROTTLE_NAMED(rate, name, ...)
#define ROS_DEBUG_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args)
#define ROS_DEBUG_FILTER(filter, ...)
#define ROS_DEBUG_STREAM_FILTER(filter, args)
#define ROS_DEBUG_FILTER_NAMED(filter, name, ...)
#define ROS_DEBUG_STREAM_FILTER_NAMED(filter, name, args)
#else
#define ROS_DEBUG(...) ROS_LOG(::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_DEBUG_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_DEBUG_NAMED(name, ...) ROS_LOG(::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_DEBUG_STREAM_NAMED(name, args) ROS_LOG_STREAM(::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_DEBUG_COND(cond, ...) ROS_LOG_COND(cond, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_DEBUG_STREAM_COND(cond, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_DEBUG_COND_NAMED(cond, name, ...) ROS_LOG_COND(cond, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_DEBUG_STREAM_COND_NAMED(cond, name, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_DEBUG_ONCE(...) ROS_LOG_ONCE(::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_DEBUG_STREAM_ONCE(args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_DEBUG_ONCE_NAMED(name, ...) ROS_LOG_ONCE(::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_DEBUG_STREAM_ONCE_NAMED(name, args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_DEBUG_THROTTLE(rate, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_DEBUG_STREAM_THROTTLE(rate, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_DEBUG_THROTTLE_NAMED(rate, name, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_DEBUG_STREAM_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_DEBUG_DELAYED_THROTTLE(rate, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_DEBUG_STREAM_DELAYED_THROTTLE(rate, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_DEBUG_DELAYED_THROTTLE_NAMED(rate, name, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_DEBUG_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_DEBUG_FILTER(filter, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_DEBUG_STREAM_FILTER(filter, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_DEBUG_FILTER_NAMED(filter, name, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_DEBUG_STREAM_FILTER_NAMED(filter, name, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Debug, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#endif
#if (ROSCONSOLE_MIN_SEVERITY > ROSCONSOLE_SEVERITY_INFO)
#define ROS_INFO(...)
#define ROS_INFO_STREAM(args)
#define ROS_INFO_NAMED(name, ...)
#define ROS_INFO_STREAM_NAMED(name, args)
#define ROS_INFO_COND(cond, ...)
#define ROS_INFO_STREAM_COND(cond, args)
#define ROS_INFO_COND_NAMED(cond, name, ...)
#define ROS_INFO_STREAM_COND_NAMED(cond, name, args)
#define ROS_INFO_ONCE(...)
#define ROS_INFO_STREAM_ONCE(args)
#define ROS_INFO_ONCE_NAMED(name, ...)
#define ROS_INFO_STREAM_ONCE_NAMED(name, args)
#define ROS_INFO_THROTTLE(rate, ...)
#define ROS_INFO_STREAM_THROTTLE(rate, args)
#define ROS_INFO_THROTTLE_NAMED(rate, name, ...)
#define ROS_INFO_STREAM_THROTTLE_NAMED(rate, name, args)
#define ROS_INFO_DELAYED_THROTTLE(rate, ...)
#define ROS_INFO_STREAM_DELAYED_THROTTLE(rate, args)
#define ROS_INFO_DELAYED_THROTTLE_NAMED(rate, name, ...)
#define ROS_INFO_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args)
#define ROS_INFO_FILTER(filter, ...)
#define ROS_INFO_STREAM_FILTER(filter, args)
#define ROS_INFO_FILTER_NAMED(filter, name, ...)
#define ROS_INFO_STREAM_FILTER_NAMED(filter, name, args)
#else
#define ROS_INFO(...) ROS_LOG(::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_INFO_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_INFO_NAMED(name, ...) ROS_LOG(::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_INFO_STREAM_NAMED(name, args) ROS_LOG_STREAM(::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_INFO_COND(cond, ...) ROS_LOG_COND(cond, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_INFO_STREAM_COND(cond, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_INFO_COND_NAMED(cond, name, ...) ROS_LOG_COND(cond, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_INFO_STREAM_COND_NAMED(cond, name, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_INFO_ONCE(...) ROS_LOG_ONCE(::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_INFO_STREAM_ONCE(args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_INFO_ONCE_NAMED(name, ...) ROS_LOG_ONCE(::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_INFO_STREAM_ONCE_NAMED(name, args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_INFO_THROTTLE(rate, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_INFO_STREAM_THROTTLE(rate, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_INFO_THROTTLE_NAMED(rate, name, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_INFO_STREAM_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_INFO_DELAYED_THROTTLE(rate, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_INFO_STREAM_DELAYED_THROTTLE(rate, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_INFO_DELAYED_THROTTLE_NAMED(rate, name, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_INFO_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_INFO_FILTER(filter, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_INFO_STREAM_FILTER(filter, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Info, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_INFO_FILTER_NAMED(filter, name, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_INFO_STREAM_FILTER_NAMED(filter, name, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Info, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#endif
#if (ROSCONSOLE_MIN_SEVERITY > ROSCONSOLE_SEVERITY_WARN)
#define ROS_WARN(...)
#define ROS_WARN_STREAM(args)
#define ROS_WARN_NAMED(name, ...)
#define ROS_WARN_STREAM_NAMED(name, args)
#define ROS_WARN_COND(cond, ...)
#define ROS_WARN_STREAM_COND(cond, args)
#define ROS_WARN_COND_NAMED(cond, name, ...)
#define ROS_WARN_STREAM_COND_NAMED(cond, name, args)
#define ROS_WARN_ONCE(...)
#define ROS_WARN_STREAM_ONCE(args)
#define ROS_WARN_ONCE_NAMED(name, ...)
#define ROS_WARN_STREAM_ONCE_NAMED(name, args)
#define ROS_WARN_THROTTLE(rate, ...)
#define ROS_WARN_STREAM_THROTTLE(rate, args)
#define ROS_WARN_THROTTLE_NAMED(rate, name, ...)
#define ROS_WARN_STREAM_THROTTLE_NAMED(rate, name, args)
#define ROS_WARN_DELAYED_THROTTLE(rate, ...)
#define ROS_WARN_STREAM_DELAYED_THROTTLE(rate, args)
#define ROS_WARN_DELAYED_THROTTLE_NAMED(rate, name, ...)
#define ROS_WARN_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args)
#define ROS_WARN_FILTER(filter, ...)
#define ROS_WARN_STREAM_FILTER(filter, args)
#define ROS_WARN_FILTER_NAMED(filter, name, ...)
#define ROS_WARN_STREAM_FILTER_NAMED(filter, name, args)
#else
#define ROS_WARN(...) ROS_LOG(::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_WARN_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_WARN_NAMED(name, ...) ROS_LOG(::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_WARN_STREAM_NAMED(name, args) ROS_LOG_STREAM(::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_WARN_COND(cond, ...) ROS_LOG_COND(cond, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_WARN_STREAM_COND(cond, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_WARN_COND_NAMED(cond, name, ...) ROS_LOG_COND(cond, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_WARN_STREAM_COND_NAMED(cond, name, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_WARN_ONCE(...) ROS_LOG_ONCE(::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_WARN_STREAM_ONCE(args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_WARN_ONCE_NAMED(name, ...) ROS_LOG_ONCE(::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_WARN_STREAM_ONCE_NAMED(name, args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_WARN_THROTTLE(rate, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_WARN_STREAM_THROTTLE(rate, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_WARN_THROTTLE_NAMED(rate, name, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_WARN_STREAM_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_WARN_DELAYED_THROTTLE(rate, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_WARN_STREAM_DELAYED_THROTTLE(rate, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_WARN_DELAYED_THROTTLE_NAMED(rate, name, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_WARN_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_WARN_FILTER(filter, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_WARN_STREAM_FILTER(filter, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Warn, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_WARN_FILTER_NAMED(filter, name, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_WARN_STREAM_FILTER_NAMED(filter, name, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Warn, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#endif
#if (ROSCONSOLE_MIN_SEVERITY > ROSCONSOLE_SEVERITY_ERROR)
#define ROS_ERROR(...)
#define ROS_ERROR_STREAM(args)
#define ROS_ERROR_NAMED(name, ...)
#define ROS_ERROR_STREAM_NAMED(name, args)
#define ROS_ERROR_COND(cond, ...)
#define ROS_ERROR_STREAM_COND(cond, args)
#define ROS_ERROR_COND_NAMED(cond, name, ...)
#define ROS_ERROR_STREAM_COND_NAMED(cond, name, args)
#define ROS_ERROR_ONCE(...)
#define ROS_ERROR_STREAM_ONCE(args)
#define ROS_ERROR_ONCE_NAMED(name, ...)
#define ROS_ERROR_STREAM_ONCE_NAMED(name, args)
#define ROS_ERROR_THROTTLE(rate, ...)
#define ROS_ERROR_STREAM_THROTTLE(rate, args)
#define ROS_ERROR_THROTTLE_NAMED(rate, name, ...)
#define ROS_ERROR_STREAM_THROTTLE_NAMED(rate, name, args)
#define ROS_ERROR_DELAYED_THROTTLE(rate, ...)
#define ROS_ERROR_STREAM_DELAYED_THROTTLE(rate, args)
#define ROS_ERROR_DELAYED_THROTTLE_NAMED(rate, name, ...)
#define ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args)
#define ROS_ERROR_FILTER(filter, ...)
#define ROS_ERROR_STREAM_FILTER(filter, args)
#define ROS_ERROR_FILTER_NAMED(filter, name, ...)
#define ROS_ERROR_STREAM_FILTER_NAMED(filter, name, args)
#else
#define ROS_ERROR(...) ROS_LOG(::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_ERROR_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_ERROR_NAMED(name, ...) ROS_LOG(::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_ERROR_STREAM_NAMED(name, args) ROS_LOG_STREAM(::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_ERROR_COND(cond, ...) ROS_LOG_COND(cond, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_ERROR_STREAM_COND(cond, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_ERROR_COND_NAMED(cond, name, ...) ROS_LOG_COND(cond, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_ERROR_STREAM_COND_NAMED(cond, name, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_ERROR_ONCE(...) ROS_LOG_ONCE(::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_ERROR_STREAM_ONCE(args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_ERROR_ONCE_NAMED(name, ...) ROS_LOG_ONCE(::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_ERROR_STREAM_ONCE_NAMED(name, args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_ERROR_THROTTLE(rate, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_ERROR_STREAM_THROTTLE(rate, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_ERROR_THROTTLE_NAMED(rate, name, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_ERROR_STREAM_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_ERROR_DELAYED_THROTTLE(rate, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_ERROR_STREAM_DELAYED_THROTTLE(rate, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_ERROR_DELAYED_THROTTLE_NAMED(rate, name, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_ERROR_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_ERROR_FILTER(filter, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_ERROR_STREAM_FILTER(filter, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Error, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_ERROR_FILTER_NAMED(filter, name, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_ERROR_STREAM_FILTER_NAMED(filter, name, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Error, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#endif
#if (ROSCONSOLE_MIN_SEVERITY > ROSCONSOLE_SEVERITY_FATAL)
#define ROS_FATAL(...)
#define ROS_FATAL_STREAM(args)
#define ROS_FATAL_NAMED(name, ...)
#define ROS_FATAL_STREAM_NAMED(name, args)
#define ROS_FATAL_COND(cond, ...)
#define ROS_FATAL_STREAM_COND(cond, args)
#define ROS_FATAL_COND_NAMED(cond, name, ...)
#define ROS_FATAL_STREAM_COND_NAMED(cond, name, args)
#define ROS_FATAL_ONCE(...)
#define ROS_FATAL_STREAM_ONCE(args)
#define ROS_FATAL_ONCE_NAMED(name, ...)
#define ROS_FATAL_STREAM_ONCE_NAMED(name, args)
#define ROS_FATAL_THROTTLE(rate, ...)
#define ROS_FATAL_STREAM_THROTTLE(rate, args)
#define ROS_FATAL_THROTTLE_NAMED(rate, name, ...)
#define ROS_FATAL_STREAM_THROTTLE_NAMED(rate, name, args)
#define ROS_FATAL_DELAYED_THROTTLE(rate, ...)
#define ROS_FATAL_STREAM_DELAYED_THROTTLE(rate, args)
#define ROS_FATAL_DELAYED_THROTTLE_NAMED(rate, name, ...)
#define ROS_FATAL_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args)
#define ROS_FATAL_FILTER(filter, ...)
#define ROS_FATAL_STREAM_FILTER(filter, args)
#define ROS_FATAL_FILTER_NAMED(filter, name, ...)
#define ROS_FATAL_STREAM_FILTER_NAMED(filter, name, args)
#else
#define ROS_FATAL(...) ROS_LOG(::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_FATAL_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_FATAL_NAMED(name, ...) ROS_LOG(::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_FATAL_STREAM_NAMED(name, args) ROS_LOG_STREAM(::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_FATAL_COND(cond, ...) ROS_LOG_COND(cond, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_FATAL_STREAM_COND(cond, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_FATAL_COND_NAMED(cond, name, ...) ROS_LOG_COND(cond, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_FATAL_STREAM_COND_NAMED(cond, name, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_FATAL_ONCE(...) ROS_LOG_ONCE(::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_FATAL_STREAM_ONCE(args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_FATAL_ONCE_NAMED(name, ...) ROS_LOG_ONCE(::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_FATAL_STREAM_ONCE_NAMED(name, args) ROS_LOG_STREAM_ONCE(::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_FATAL_THROTTLE(rate, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_FATAL_STREAM_THROTTLE(rate, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_FATAL_THROTTLE_NAMED(rate, name, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_FATAL_STREAM_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_FATAL_DELAYED_THROTTLE(rate, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_FATAL_STREAM_DELAYED_THROTTLE(rate, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_FATAL_DELAYED_THROTTLE_NAMED(rate, name, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_FATAL_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#define ROS_FATAL_FILTER(filter, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)
#define ROS_FATAL_STREAM_FILTER(filter, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Fatal, ROSCONSOLE_DEFAULT_NAME, args)
#define ROS_FATAL_FILTER_NAMED(filter, name, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)
#define ROS_FATAL_STREAM_FILTER_NAMED(filter, name, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::Fatal, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)
#endif

11
thirdparty/ros/ros_comm/tools/rosconsole/mainpage.dox vendored Normal file
View File

@@ -0,0 +1,11 @@
/**
* \mainpage
*
* \htmlinclude manifest.html
*
* \b rosconsole is a package for console output and logging. It provides a macro-based interface
* which allows both printf- and stream-style output. It also wraps log4cxx (http://logging.apache.org/log4cxx/index.html),
* which supports hierarchical loggers, verbosity levels and configuration-files.
*
*/

25
thirdparty/ros/ros_comm/tools/rosconsole/package.xml vendored Normal file
View File

@@ -0,0 +1,25 @@
<package>
<name>rosconsole</name>
<version>1.12.14</version>
<description>ROS console output library.</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<url>http://www.ros.org/wiki/rosconsole</url>
<author>Josh Faust</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>apr</build_depend>
<build_depend>boost</build_depend>
<build_depend>cpp_common</build_depend>
<build_depend>log4cxx</build_depend>
<build_depend>rostime</build_depend>
<build_depend>rosunit</build_depend>
<run_depend>apr</run_depend>
<run_depend>cpp_common</run_depend>
<run_depend>log4cxx</run_depend>
<run_depend>rosbuild</run_depend>
<run_depend>rostime</run_depend>
</package>

137
thirdparty/ros/ros_comm/tools/rosconsole/scripts/generate_macros.py vendored Executable file
View File

@@ -0,0 +1,137 @@
#!/usr/bin/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 os
import sys
def add_macro(f, caps_name, enum_name):
f.write('#if (ROSCONSOLE_MIN_SEVERITY > ROSCONSOLE_SEVERITY_%s)\n' %(caps_name))
f.write('#define ROS_%s(...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM(args)\n' %(caps_name))
f.write('#define ROS_%s_NAMED(name, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_NAMED(name, args)\n' %(caps_name))
f.write('#define ROS_%s_COND(cond, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_COND(cond, args)\n' %(caps_name))
f.write('#define ROS_%s_COND_NAMED(cond, name, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_COND_NAMED(cond, name, args)\n' %(caps_name))
f.write('#define ROS_%s_ONCE(...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_ONCE(args)\n' %(caps_name))
f.write('#define ROS_%s_ONCE_NAMED(name, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_ONCE_NAMED(name, args)\n' %(caps_name))
f.write('#define ROS_%s_THROTTLE(rate, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_THROTTLE(rate, args)\n' %(caps_name))
f.write('#define ROS_%s_THROTTLE_NAMED(rate, name, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_THROTTLE_NAMED(rate, name, args)\n' %(caps_name))
f.write('#define ROS_%s_DELAYED_THROTTLE(rate, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_DELAYED_THROTTLE(rate, args)\n' %(caps_name))
f.write('#define ROS_%s_DELAYED_THROTTLE_NAMED(rate, name, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args)\n' %(caps_name))
f.write('#define ROS_%s_FILTER(filter, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_FILTER(filter, args)\n' %(caps_name))
f.write('#define ROS_%s_FILTER_NAMED(filter, name, ...)\n' %(caps_name))
f.write('#define ROS_%s_STREAM_FILTER_NAMED(filter, name, args)\n' %(caps_name))
f.write('#else\n')
f.write('#define ROS_%s(...) ROS_LOG(::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_NAMED(name, ...) ROS_LOG(::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_NAMED(name, args) ROS_LOG_STREAM(::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_COND(cond, ...) ROS_LOG_COND(cond, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_COND(cond, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_COND_NAMED(cond, name, ...) ROS_LOG_COND(cond, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_COND_NAMED(cond, name, args) ROS_LOG_STREAM_COND(cond, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_ONCE(...) ROS_LOG_ONCE(::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_ONCE(args) ROS_LOG_STREAM_ONCE(::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_ONCE_NAMED(name, ...) ROS_LOG_ONCE(::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_ONCE_NAMED(name, args) ROS_LOG_STREAM_ONCE(::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_THROTTLE(rate, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_THROTTLE(rate, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_THROTTLE_NAMED(rate, name, ...) ROS_LOG_THROTTLE(rate, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_THROTTLE(rate, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_DELAYED_THROTTLE(rate, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_DELAYED_THROTTLE(rate, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_DELAYED_THROTTLE_NAMED(rate, name, ...) ROS_LOG_DELAYED_THROTTLE(rate, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_DELAYED_THROTTLE_NAMED(rate, name, args) ROS_LOG_STREAM_DELAYED_THROTTLE(rate, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_FILTER(filter, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_FILTER(filter, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::%s, ROSCONSOLE_DEFAULT_NAME, args)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_FILTER_NAMED(filter, name, ...) ROS_LOG_FILTER(filter, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, __VA_ARGS__)\n' %(caps_name, enum_name))
f.write('#define ROS_%s_STREAM_FILTER_NAMED(filter, name, args) ROS_LOG_STREAM_FILTER(filter, ::ros::console::levels::%s, std::string(ROSCONSOLE_NAME_PREFIX) + "." + name, args)\n' %(caps_name, enum_name))
f.write('#endif\n\n')
f = open('%s/../include/rosconsole/macros_generated.h' %(os.path.dirname(__file__)), 'w')
f.write("// !!!!!!!!!!!!!!!!!!!!!!! This is a generated file, do not edit manually\n\n")
f.write('/*\n')
f.write(' * Copyright (c) 2008, Willow Garage, Inc.\n')
f.write(' * All rights reserved.\n')
f.write(' *\n')
f.write(' * Redistribution and use in source and binary forms, with or without\n')
f.write(' * modification, are permitted provided that the following conditions are met:\n')
f.write(' *\n')
f.write(' * * Redistributions of source code must retain the above copyright\n')
f.write(' * notice, this list of conditions and the following disclaimer.\n')
f.write(' * * Redistributions in binary form must reproduce the above copyright\n')
f.write(' * notice, this list of conditions and the following disclaimer in the\n')
f.write(' * documentation and/or other materials provided with the distribution.\n')
f.write(' * * Neither the name of Willow Garage, Inc. nor the names of its\n')
f.write(' * contributors may be used to endorse or promote products derived from\n')
f.write(' * this software without specific prior written permission.\n')
f.write(' *\n')
f.write(' * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"\n')
f.write(' * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n')
f.write(' * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n')
f.write(' * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n')
f.write(' * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n')
f.write(' * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n')
f.write(' * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n')
f.write(' * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n')
f.write(' * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n')
f.write(' * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n')
f.write(' * POSSIBILITY OF SUCH DAMAGE.\n')
f.write(' */\n\n')
add_macro(f, "DEBUG", "Debug")
add_macro(f, "INFO", "Info")
add_macro(f, "WARN", "Warn")
add_macro(f, "ERROR", "Error")
add_macro(f, "FATAL", "Fatal")

106
thirdparty/ros/ros_comm/tools/rosconsole/scripts/generate_speed_test.py vendored Executable file
View File

@@ -0,0 +1,106 @@
#!/usr/bin/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 os
import sys
f = open('%s/../test/speed_test.cpp' % (os.path.dirname(__file__)), 'w')
f.write("// !!!!!!!!!!!!!!!!!!!!!!! This is a generated file, do not edit manually\n\n")
f.write('/*\n')
f.write(' * Copyright (c) 2008, Willow Garage, Inc.\n')
f.write(' * All rights reserved.\n')
f.write(' *\n')
f.write(' * Redistribution and use in source and binary forms, with or without\n')
f.write(' * modification, are permitted provided that the following conditions are met:\n')
f.write(' *\n')
f.write(' * * Redistributions of source code must retain the above copyright\n')
f.write(' * notice, this list of conditions and the following disclaimer.\n')
f.write(' * * Redistributions in binary form must reproduce the above copyright\n')
f.write(' * notice, this list of conditions and the following disclaimer in the\n')
f.write(' * documentation and/or other materials provided with the distribution.\n')
f.write(' * * Neither the name of Willow Garage, Inc. nor the names of its\n')
f.write(' * contributors may be used to endorse or promote products derived from\n')
f.write(' * this software without specific prior written permission.\n')
f.write(' *\n')
f.write(' * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"\n')
f.write(' * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE\n')
f.write(' * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE\n')
f.write(' * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE\n')
f.write(' * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR\n')
f.write(' * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF\n')
f.write(' * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS\n')
f.write(' * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN\n')
f.write(' * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)\n')
f.write(' * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE\n')
f.write(' * POSSIBILITY OF SUCH DAMAGE.\n')
f.write(' */\n\n')
f.write('#include "ros/console.h"\n')
f.write('#include "log4cxx/appenderskeleton.h"\n')
#for i in range(0,int(sys.argv[1])):
# f.write('void info%s(int i) { ROS_INFO("Info%s: %%d", i); }\n' %(i,i))
# f.write('void warn%s(int i) { ROS_WARN("Warn%s: %%d", i); }\n' %(i,i))
# f.write('void error%s(int i) { ROS_ERROR("Error%s: %%d", i); }\n' %(i,i))
# f.write('void debug%s(int i) { ROS_DEBUG("Debug%s: %%d", i); }\n' %(i,i))
# f.write('void errorr%s(int i) { ROS_ERROR("Error2%s: %%d", i); }\n' %(i,i))
f.write('class NullAppender : public log4cxx::AppenderSkeleton {\n')
f.write('protected:\n')
f.write('virtual void append(const log4cxx::spi::LoggingEventPtr& event, log4cxx::helpers::Pool& pool){printf("blah\\n");}\n')
f.write('virtual void close() {}\n')
f.write('virtual bool requiresLayout() const { return false; } };\n')
f.write('int main(int argc, char** argv)\n{\n')
f.write('ROSCONSOLE_AUTOINIT; \nlog4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME)->removeAllAppenders();\n')
f.write('log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->addAppender(new NullAppender);\n')
f.write('log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME)->setLevel(log4cxx::Level::getFatal());\n')
f.write('for (int i = 0;i < %s; ++i)\n{\n' %(sys.argv[2]))
for i in range(0,int(sys.argv[1])):
#f.write('info%s(i);\n' %(i))
#f.write('warn%s(i);\n' %(i))
#f.write('error%s(i);\n' %(i))
#f.write('debug%s(i);\n' %(i))
#f.write('errorr%s(i);\n' %(i))
f.write('ROS_INFO("test");')
f.write('ROS_WARN("test");')
f.write('ROS_ERROR("test");')
f.write('ROS_DEBUG("test");')
f.write('ROS_ERROR("test");')
f.write('}\n')
f.write('}\n')

127
thirdparty/ros/ros_comm/tools/rosconsole/src/rosconsole/impl/rosconsole_glog.cpp vendored Normal file
View File

@@ -0,0 +1,127 @@
#include "ros/console.h"
#include <glog/logging.h>
namespace ros
{
namespace console
{
namespace impl
{
std::vector<std::pair<std::string, levels::Level> > rosconsole_glog_log_levels;
LogAppender* rosconsole_glog_appender = 0;
void initialize()
{
google::InitGoogleLogging("rosconsole");
}
std::string getName(void* handle);
void print(void* handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
// still printing to console
::ros::console::backend::print(0, level, str, file, function, line);
// pass log message to appender
if(rosconsole_glog_appender)
{
rosconsole_glog_appender->log(level, str, file, function, line);
}
google::LogSeverity glog_level;
if(level == ::ros::console::levels::Info)
{
glog_level = google::GLOG_INFO;
}
else if(level == ::ros::console::levels::Warn)
{
glog_level = google::GLOG_WARNING;
}
else if(level == ::ros::console::levels::Error)
{
glog_level = google::GLOG_ERROR;
}
else if(level == ::ros::console::levels::Fatal)
{
glog_level = google::GLOG_FATAL;
}
else
{
// ignore debug
return;
}
std::string name = getName(handle);
google::LogMessage(file, line, glog_level).stream() << name << ": " << str;
}
bool isEnabledFor(void* handle, ::ros::console::Level level)
{
size_t index = (size_t)handle;
if(index < rosconsole_glog_log_levels.size())
{
return level >= rosconsole_glog_log_levels[index].second;
}
return false;
}
void* getHandle(const std::string& name)
{
size_t count = rosconsole_glog_log_levels.size();
for(size_t index = 0; index < count; index++)
{
if(name == rosconsole_glog_log_levels[index].first)
{
return (void*)index;
}
index++;
}
// add unknown names on demand with default level
rosconsole_glog_log_levels.push_back(std::pair<std::string, levels::Level>(name, ::ros::console::levels::Info));
return (void*)(rosconsole_glog_log_levels.size() - 1);
}
std::string getName(void* handle)
{
size_t index = (size_t)handle;
if(index < rosconsole_glog_log_levels.size())
{
return rosconsole_glog_log_levels[index].first;
}
return "";
}
void register_appender(LogAppender* appender)
{
rosconsole_glog_appender = appender;
}
void shutdown()
{}
bool get_loggers(std::map<std::string, levels::Level>& loggers)
{
for(std::vector<std::pair<std::string, levels::Level> >::const_iterator it = rosconsole_glog_log_levels.begin(); it != rosconsole_glog_log_levels.end(); it++)
{
loggers[it->first] = it->second;
}
return true;
}
bool set_logger_level(const std::string& name, levels::Level level)
{
for(std::vector<std::pair<std::string, levels::Level> >::iterator it = rosconsole_glog_log_levels.begin(); it != rosconsole_glog_log_levels.end(); it++)
{
if(name == it->first)
{
it->second = level;
return true;
}
}
return false;
}
} // namespace impl
} // namespace console
} // namespace ros

375
thirdparty/ros/ros_comm/tools/rosconsole/src/rosconsole/impl/rosconsole_log4cxx.cpp vendored Normal file
View File

@@ -0,0 +1,375 @@
/*
* 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 the 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.
*/
// Author: Josh Faust
#if defined(__APPLE__) && defined(__GNUC__) && defined(__llvm__) && !defined(__clang__) && (__GNUC__ == 4) && (__GNUC_MINOR__ == 2)
#error This code is known to provoke a compiler crash with llvm-gcc 4.2. You will have better luck with clang++. See code.ros.org/trac/ros/ticket/3626
#endif
#include "ros/console.h"
#include "ros/assert.h"
#include <ros/time.h>
#include "log4cxx/appenderskeleton.h"
#include "log4cxx/spi/loggingevent.h"
#include "log4cxx/level.h"
#include "log4cxx/propertyconfigurator.h"
#ifdef _MSC_VER
// Have to be able to encode wchar LogStrings on windows.
#include "log4cxx/helpers/transcoder.h"
#endif
#include <boost/thread.hpp>
#include <boost/shared_array.hpp>
#include <boost/regex.hpp>
#include <cstdarg>
#include <cstdlib>
#include <cstdio>
#include <memory>
#include <cstring>
#include <stdexcept>
namespace ros
{
namespace console
{
namespace impl
{
log4cxx::LevelPtr g_level_lookup[ levels::Count ] =
{
log4cxx::Level::getDebug(),
log4cxx::Level::getInfo(),
log4cxx::Level::getWarn(),
log4cxx::Level::getError(),
log4cxx::Level::getFatal(),
};
class ROSConsoleStdioAppender : public log4cxx::AppenderSkeleton
{
public:
~ROSConsoleStdioAppender()
{
}
protected:
virtual void append(const log4cxx::spi::LoggingEventPtr& event,
log4cxx::helpers::Pool&)
{
levels::Level level = levels::Count;
if (event->getLevel() == log4cxx::Level::getDebug())
{
level = levels::Debug;
}
else if (event->getLevel() == log4cxx::Level::getInfo())
{
level = levels::Info;
}
else if (event->getLevel() == log4cxx::Level::getWarn())
{
level = levels::Warn;
}
else if (event->getLevel() == log4cxx::Level::getError())
{
level = levels::Error;
}
else if (event->getLevel() == log4cxx::Level::getFatal())
{
level = levels::Fatal;
}
#ifdef _MSC_VER
LOG4CXX_ENCODE_CHAR(tmpstr, event->getMessage()); // has to handle LogString with wchar types.
std::string msg = tmpstr // tmpstr gets instantiated inside the LOG4CXX_ENCODE_CHAR macro
#else
std::string msg = event->getMessage();
#endif
const log4cxx::spi::LocationInfo& location_info = event->getLocationInformation();
::ros::console::backend::print(event.operator->(), level, msg.c_str(), location_info.getFileName(), location_info.getMethodName().c_str(), location_info.getLineNumber());
}
virtual void close()
{
}
virtual bool requiresLayout() const
{
return false;
}
};
void initialize()
{
// First set up some sane defaults programmatically.
log4cxx::LoggerPtr ros_logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
ros_logger->setLevel(log4cxx::Level::getInfo());
log4cxx::LoggerPtr roscpp_superdebug = log4cxx::Logger::getLogger("ros.roscpp.superdebug");
roscpp_superdebug->setLevel(log4cxx::Level::getWarn());
// Next try to load the default config file from ROS_ROOT/config/rosconsole.config
char* ros_root_cstr = NULL;
#ifdef _MSC_VER
_dupenv_s(&ros_root_cstr, NULL, "ROS_ROOT");
#else
ros_root_cstr = getenv("ROS_ROOT");
#endif
if (ros_root_cstr)
{
std::string config_file = std::string(ros_root_cstr) + "/config/rosconsole.config";
FILE* config_file_ptr = fopen( config_file.c_str(), "r" );
if( config_file_ptr ) // only load it if the file exists, to avoid a warning message getting printed.
{
fclose( config_file_ptr );
log4cxx::PropertyConfigurator::configure(config_file);
}
}
char* config_file_cstr = NULL;
#ifdef _MSC_VER
_dupenv_s(&config_file_cstr, NULL, "ROSCONSOLE_CONFIG_FILE");
#else
config_file_cstr = getenv("ROSCONSOLE_CONFIG_FILE");
#endif
if ( config_file_cstr )
{
std::string config_file = config_file_cstr;
log4cxx::PropertyConfigurator::configure(config_file);
}
log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
logger->addAppender(new ROSConsoleStdioAppender);
#ifdef _MSC_VER
if ( ros_root_cstr != NULL ) {
free(ros_root_cstr);
}
if ( config_file_cstr != NULL ) {
free(config_file_cstr);
}
if ( format_string != NULL ) {
free(format_string);
}
// getenv implementations don't need free'ing.
#endif
}
void print(void* handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
log4cxx::Logger* logger = (log4cxx::Logger*)handle;
try
{
logger->forcedLog(g_level_lookup[level], str, log4cxx::spi::LocationInfo(file, function, line));
}
catch (std::exception& e)
{
fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
}
}
bool isEnabledFor(void* handle, ::ros::console::Level level)
{
log4cxx::Logger* logger = (log4cxx::Logger*)handle;
return logger->isEnabledFor(g_level_lookup[level]);
}
void* getHandle(const std::string& name)
{
return log4cxx::Logger::getLogger(name);
}
std::string getName(void* handle)
{
const log4cxx::spi::LoggingEvent* event = (const log4cxx::spi::LoggingEvent*)handle;
#ifdef _MSC_VER
LOG4CXX_ENCODE_CHAR(tmpstr, event->getLoggerName()); // has to handle LogString with wchar types.
return tmpstr // tmpstr gets instantiated inside the LOG4CXX_ENCODE_CHAR macro
#else
return event->getLoggerName();
#endif
}
bool get_loggers(std::map<std::string, levels::Level>& loggers)
{
log4cxx::spi::LoggerRepositoryPtr repo = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME)->getLoggerRepository();
log4cxx::LoggerList current_loggers = repo->getCurrentLoggers();
log4cxx::LoggerList::iterator it = current_loggers.begin();
log4cxx::LoggerList::iterator end = current_loggers.end();
for (; it != end; ++it)
{
std::string name;
#ifdef _MSC_VER
LOG4CXX_ENCODE_CHAR(name, (*it)->getName()); // has to handle LogString with wchar types.
#else
name = (*it)->getName();
#endif
const log4cxx::LevelPtr& log4cxx_level = (*it)->getEffectiveLevel();
levels::Level level;
if (log4cxx_level == log4cxx::Level::getDebug())
{
level = levels::Debug;
}
else if (log4cxx_level == log4cxx::Level::getInfo())
{
level = levels::Info;
}
else if (log4cxx_level == log4cxx::Level::getWarn())
{
level = levels::Warn;
}
else if (log4cxx_level == log4cxx::Level::getError())
{
level = levels::Error;
}
else if (log4cxx_level == log4cxx::Level::getFatal())
{
level = levels::Fatal;
}
else
{
return false;
}
loggers[name] = level;
}
return true;
}
bool set_logger_level(const std::string& name, levels::Level level)
{
log4cxx::LevelPtr log4cxx_level;
if (level == levels::Debug)
{
log4cxx_level = log4cxx::Level::getDebug();
}
else if (level == levels::Info)
{
log4cxx_level = log4cxx::Level::getInfo();
}
else if (level == levels::Warn)
{
log4cxx_level = log4cxx::Level::getWarn();
}
else if (level == levels::Error)
{
log4cxx_level = log4cxx::Level::getError();
}
else if (level == levels::Fatal)
{
log4cxx_level = log4cxx::Level::getFatal();
}
else
{
return false;
}
log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(name);
logger->setLevel(log4cxx_level);
::ros::console::backend::notifyLoggerLevelsChanged();
return true;
}
class Log4cxxAppender : public log4cxx::AppenderSkeleton
{
public:
Log4cxxAppender(ros::console::LogAppender* appender) : appender_(appender) {}
~Log4cxxAppender() {}
protected:
virtual void append(const log4cxx::spi::LoggingEventPtr& event, log4cxx::helpers::Pool& pool)
{
(void)pool;
levels::Level level;
if (event->getLevel() == log4cxx::Level::getFatal())
{
level = levels::Fatal;
}
else if (event->getLevel() == log4cxx::Level::getError())
{
level = levels::Error;
}
else if (event->getLevel() == log4cxx::Level::getWarn())
{
level = levels::Warn;
}
else if (event->getLevel() == log4cxx::Level::getInfo())
{
level = levels::Info;
}
else if (event->getLevel() == log4cxx::Level::getDebug())
{
level = levels::Debug;
}
else
{
return;
}
#ifdef _MSC_VER
LOG4CXX_ENCODE_CHAR(tmpstr, event->getMessage()); // has to handle LogString with wchar types.
std::string msg = tmpstr // tmpstr gets instantiated inside the LOG4CXX_ENCODE_CHAR macro
#else
std::string msg = event->getMessage();
#endif
const log4cxx::spi::LocationInfo& info = event->getLocationInformation();
appender_->log(level, msg.c_str(), info.getFileName(), info.getMethodName().c_str(), info.getLineNumber());
}
virtual void close() {}
virtual bool requiresLayout() const { return false; }
ros::console::LogAppender* appender_;
};
Log4cxxAppender* g_log4cxx_appender;
void register_appender(LogAppender* appender)
{
g_log4cxx_appender = new Log4cxxAppender(appender);
const log4cxx::LoggerPtr& logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
logger->addAppender(g_log4cxx_appender);
}
void shutdown()
{
const log4cxx::LoggerPtr& logger = log4cxx::Logger::getLogger(ROSCONSOLE_ROOT_LOGGER_NAME);
logger->removeAppender(g_log4cxx_appender);
g_log4cxx_appender = 0;
// reset this so that the logger doesn't get crashily destroyed
// again during global destruction.
//
// See https://code.ros.org/trac/ros/ticket/3271
//
log4cxx::Logger::getRootLogger()->getLoggerRepository()->shutdown();
}
} // namespace impl
} // namespace console
} // namespace ros

88
thirdparty/ros/ros_comm/tools/rosconsole/src/rosconsole/impl/rosconsole_print.cpp vendored Normal file
View File

@@ -0,0 +1,88 @@
/*
* Copyright (c) 2013, Open Source Robotics Foundation
* 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 the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ros/console.h"
namespace ros
{
namespace console
{
namespace impl
{
LogAppender* rosconsole_print_appender = 0;
void initialize()
{}
void print(void* handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
::ros::console::backend::print(0, level, str, file, function, line);
if(rosconsole_print_appender)
{
rosconsole_print_appender->log(level, str, file, function, line);
}
}
bool isEnabledFor(void* handle, ::ros::console::Level level)
{
return level != ::ros::console::levels::Debug;
}
void* getHandle(const std::string& name)
{
return 0;
}
std::string getName(void* handle)
{
return "";
}
void register_appender(LogAppender* appender)
{
rosconsole_print_appender = appender;
}
void shutdown()
{}
bool get_loggers(std::map<std::string, levels::Level>& loggers)
{
return true;
}
bool set_logger_level(const std::string& name, levels::Level level)
{
return false;
}
} // namespace impl
} // namespace console
} // namespace ros

707
thirdparty/ros/ros_comm/tools/rosconsole/src/rosconsole/rosconsole.cpp vendored Normal file
View File

@@ -0,0 +1,707 @@
/*
* 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 the 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.
*/
// Author: Josh Faust
#if defined(__APPLE__) && defined(__GNUC__) && defined(__llvm__) && !defined(__clang__) && (__GNUC__ == 4) && (__GNUC_MINOR__ == 2)
#error This code is known to provoke a compiler crash with llvm-gcc 4.2. You will have better luck with clang++. See code.ros.org/trac/ros/ticket/3626
#endif
#include "ros/console.h"
#include "ros/assert.h"
#include <ros/time.h>
#include <boost/thread.hpp>
#include <boost/shared_array.hpp>
#include <boost/regex.hpp>
#include <boost/make_shared.hpp>
#include <cstdarg>
#include <cstdlib>
#include <cstdio>
#include <memory>
#include <cstring>
#include <stdexcept>
// declare interface for rosconsole implementations
namespace ros
{
namespace console
{
namespace impl
{
void initialize();
void shutdown();
void register_appender(LogAppender* appender);
void print(void* handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line);
bool isEnabledFor(void* handle, ::ros::console::Level level);
void* getHandle(const std::string& name);
std::string getName(void* handle);
bool get_loggers(std::map<std::string, levels::Level>& loggers);
bool set_logger_level(const std::string& name, levels::Level level);
} // namespace impl
bool g_initialized = false;
bool g_shutting_down = false;
boost::mutex g_init_mutex;
#ifdef ROSCONSOLE_BACKEND_LOG4CXX
log4cxx::LevelPtr g_level_lookup[levels::Count] =
{
log4cxx::Level::getDebug(),
log4cxx::Level::getInfo(),
log4cxx::Level::getWarn(),
log4cxx::Level::getError(),
log4cxx::Level::getFatal(),
};
#endif
std::string g_last_error_message = "Unknown Error";
#ifdef WIN32
#define COLOR_NORMAL ""
#define COLOR_RED ""
#define COLOR_GREEN ""
#define COLOR_YELLOW ""
#else
#define COLOR_NORMAL "\033[0m"
#define COLOR_RED "\033[31m"
#define COLOR_GREEN "\033[32m"
#define COLOR_YELLOW "\033[33m"
#endif
const char* g_format_string = "[${severity}] [${time}]: ${message}";
typedef std::map<std::string, std::string> M_string;
M_string g_extra_fixed_tokens;
void setFixedFilterToken(const std::string& key, const std::string& val)
{
g_extra_fixed_tokens[key] = val;
}
struct FixedToken : public Token
{
FixedToken(const std::string& str)
: str_(str)
{}
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
{
return str_.c_str();
}
std::string str_;
};
struct FixedMapToken : public Token
{
FixedMapToken(const std::string& str)
: str_(str)
{}
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
{
M_string::iterator it = g_extra_fixed_tokens.find(str_);
if (it == g_extra_fixed_tokens.end())
{
return ("${" + str_ + "}").c_str();
}
return it->second.c_str();
}
std::string str_;
};
struct PlaceHolderToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
{
return "PLACEHOLDER";
}
};
struct SeverityToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
(void)str;
(void)file;
(void)function;
(void)line;
if (level == levels::Fatal)
{
return "FATAL";
}
else if (level == levels::Error)
{
return "ERROR";
}
else if (level == levels::Warn)
{
return " WARN";
}
else if (level == levels::Info)
{
return " INFO";
}
else if (level == levels::Debug)
{
return "DEBUG";
}
return "UNKNO";
}
};
struct MessageToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char* str, const char*, const char*, int)
{
return str;
}
};
struct TimeToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
{
std::stringstream ss;
ss << ros::WallTime::now();
if (ros::Time::isValid() && ros::Time::isSimTime())
{
ss << ", " << ros::Time::now();
}
return ss.str();
}
};
struct WallTimeToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
{
std::stringstream ss;
ss << ros::WallTime::now();
return ss.str();
}
};
struct ThreadToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int)
{
std::stringstream ss;
ss << boost::this_thread::get_id();
return ss.str();
}
};
struct LoggerToken : public Token
{
virtual std::string getString(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
(void)level;
(void)str;
(void)file;
(void)function;
(void)line;
return ::ros::console::impl::getName(logger_handle);
}
};
struct FileToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char* file, const char*, int)
{
return file;
}
};
struct FunctionToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char* function, int)
{
return function;
}
};
struct LineToken : public Token
{
virtual std::string getString(void*, ::ros::console::Level, const char*, const char*, const char*, int line)
{
std::stringstream ss;
ss << line;
return ss.str();
}
};
TokenPtr createTokenFromType(const std::string& type)
{
if (type == "severity")
{
return TokenPtr(boost::make_shared<SeverityToken>());
}
else if (type == "message")
{
return TokenPtr(boost::make_shared<MessageToken>());
}
else if (type == "time")
{
return TokenPtr(boost::make_shared<TimeToken>());
}
else if (type == "walltime")
{
return TokenPtr(boost::make_shared<WallTimeToken>());
}
else if (type == "thread")
{
return TokenPtr(boost::make_shared<ThreadToken>());
}
else if (type == "logger")
{
return TokenPtr(boost::make_shared<LoggerToken>());
}
else if (type == "file")
{
return TokenPtr(boost::make_shared<FileToken>());
}
else if (type == "line")
{
return TokenPtr(boost::make_shared<LineToken>());
}
else if (type == "function")
{
return TokenPtr(boost::make_shared<FunctionToken>());
}
return TokenPtr(boost::make_shared<FixedMapToken>(type));
}
void Formatter::init(const char* fmt)
{
format_ = fmt;
boost::regex e("\\$\\{([a-z|A-Z]+)\\}");
boost::match_results<std::string::const_iterator> results;
std::string::const_iterator start, end;
start = format_.begin();
end = format_.end();
bool matched_once = false;
std::string last_suffix;
while (boost::regex_search(start, end, results, e))
{
#if 0
for (size_t i = 0; i < results.size(); ++i)
{
std::cout << i << "|" << results.prefix() << "|" << results[i] << "|" << results.suffix() << std::endl;
}
#endif
std::string token = results[1];
last_suffix = results.suffix();
tokens_.push_back(TokenPtr(boost::make_shared<FixedToken>(results.prefix())));
tokens_.push_back(createTokenFromType(token));
start = results[0].second;
matched_once = true;
}
if (matched_once)
{
tokens_.push_back(TokenPtr(boost::make_shared<FixedToken>(last_suffix)));
}
else
{
tokens_.push_back(TokenPtr(boost::make_shared<FixedToken>(format_)));
}
}
void Formatter::print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
const char* color = NULL;
FILE* f = stdout;
if (level == levels::Fatal)
{
color = COLOR_RED;
f = stderr;
}
else if (level == levels::Error)
{
color = COLOR_RED;
f = stderr;
}
else if (level == levels::Warn)
{
color = COLOR_YELLOW;
}
else if (level == levels::Info)
{
color = COLOR_NORMAL;
}
else if (level == levels::Debug)
{
color = COLOR_GREEN;
}
ROS_ASSERT(color != NULL);
std::stringstream ss;
ss << color;
V_Token::iterator it = tokens_.begin();
V_Token::iterator end = tokens_.end();
for (; it != end; ++it)
{
ss << (*it)->getString(logger_handle, level, str, file, function, line);
}
ss << COLOR_NORMAL;
fprintf(f, "%s\n", ss.str().c_str());
}
Formatter g_formatter;
void _print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
g_formatter.print(logger_handle, level, str, file, function, line);
}
void initialize()
{
boost::mutex::scoped_lock lock(g_init_mutex);
if (!g_initialized)
{
// Check for the format string environment variable
char* format_string = NULL;
#ifdef _MSC_VER
_dupenv_s(&format_string, NULL, "ROSCONSOLE_FORMAT");
#else
format_string = getenv("ROSCONSOLE_FORMAT");
#endif
if (format_string)
{
g_format_string = format_string;
}
g_formatter.init(g_format_string);
backend::function_notifyLoggerLevelsChanged = notifyLoggerLevelsChanged;
backend::function_print = _print;
::ros::console::impl::initialize();
g_initialized = true;
}
}
void vformatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, va_list args)
{
#ifdef _MSC_VER
va_list arg_copy = args; // dangerous?
#else
va_list arg_copy;
va_copy(arg_copy, args);
#endif
#ifdef _MSC_VER
size_t total = vsnprintf_s(buffer.get(), buffer_size, buffer_size, fmt, args);
#else
size_t total = vsnprintf(buffer.get(), buffer_size, fmt, args);
#endif
if (total >= buffer_size)
{
buffer_size = total + 1;
buffer.reset(new char[buffer_size]);
#ifdef _MSC_VER
vsnprintf_s(buffer.get(), buffer_size, buffer_size, fmt, arg_copy);
#else
vsnprintf(buffer.get(), buffer_size, fmt, arg_copy);
#endif
}
va_end(arg_copy);
}
void formatToBuffer(boost::shared_array<char>& buffer, size_t& buffer_size, const char* fmt, ...)
{
va_list args;
va_start(args, fmt);
vformatToBuffer(buffer, buffer_size, fmt, args);
va_end(args);
}
std::string formatToString(const char* fmt, ...)
{
boost::shared_array<char> buffer;
size_t size = 0;
va_list args;
va_start(args, fmt);
vformatToBuffer(buffer, size, fmt, args);
va_end(args);
return std::string(buffer.get(), size);
}
#define INITIAL_BUFFER_SIZE 4096
static boost::mutex g_print_mutex;
static boost::shared_array<char> g_print_buffer(new char[INITIAL_BUFFER_SIZE]);
static size_t g_print_buffer_size = INITIAL_BUFFER_SIZE;
static boost::thread::id g_printing_thread_id;
void print(FilterBase* filter, void* logger_handle, Level level,
const char* file, int line, const char* function, const char* fmt, ...)
{
if (g_shutting_down)
return;
if (g_printing_thread_id == boost::this_thread::get_id())
{
fprintf(stderr, "Warning: recursive print statement has occurred. Throwing out recursive print.\n");
return;
}
boost::mutex::scoped_lock lock(g_print_mutex);
g_printing_thread_id = boost::this_thread::get_id();
va_list args;
va_start(args, fmt);
vformatToBuffer(g_print_buffer, g_print_buffer_size, fmt, args);
va_end(args);
bool enabled = true;
if (filter)
{
FilterParams params;
params.file = file;
params.function = function;
params.line = line;
params.level = level;
params.logger = logger_handle;
params.message = g_print_buffer.get();
enabled = filter->isEnabled(params);
level = params.level;
if (!params.out_message.empty())
{
size_t msg_size = params.out_message.size();
if (g_print_buffer_size <= msg_size)
{
g_print_buffer_size = msg_size + 1;
g_print_buffer.reset(new char[g_print_buffer_size]);
}
memcpy(g_print_buffer.get(), params.out_message.c_str(), msg_size + 1);
}
}
if (enabled)
{
if (level == levels::Error)
{
g_last_error_message = g_print_buffer.get();
}
try
{
::ros::console::impl::print(logger_handle, level, g_print_buffer.get(), file, function, line);
}
catch (std::exception& e)
{
fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
}
}
g_printing_thread_id = boost::thread::id();
}
void print(FilterBase* filter, void* logger_handle, Level level,
const std::stringstream& ss, const char* file, int line, const char* function)
{
if (g_shutting_down)
return;
if (g_printing_thread_id == boost::this_thread::get_id())
{
fprintf(stderr, "Warning: recursive print statement has occurred. Throwing out recursive print.\n");
return;
}
boost::mutex::scoped_lock lock(g_print_mutex);
g_printing_thread_id = boost::this_thread::get_id();
bool enabled = true;
std::string str = ss.str();
if (filter)
{
FilterParams params;
params.file = file;
params.function = function;
params.line = line;
params.level = level;
params.logger = logger_handle;
params.message = g_print_buffer.get();
enabled = filter->isEnabled(params);
level = params.level;
if (!params.out_message.empty())
{
str = params.out_message;
}
}
if (enabled)
{
if (level == levels::Error)
{
g_last_error_message = str;
}
try
{
::ros::console::impl::print(logger_handle, level, str.c_str(), file, function, line);
}
catch (std::exception& e)
{
fprintf(stderr, "Caught exception while logging: [%s]\n", e.what());
}
}
g_printing_thread_id = boost::thread::id();
}
typedef std::vector<LogLocation*> V_LogLocation;
V_LogLocation g_log_locations;
boost::mutex g_locations_mutex;
void registerLogLocation(LogLocation* loc)
{
boost::mutex::scoped_lock lock(g_locations_mutex);
g_log_locations.push_back(loc);
}
void checkLogLocationEnabledNoLock(LogLocation* loc)
{
loc->logger_enabled_ = ::ros::console::impl::isEnabledFor(loc->logger_, loc->level_);
}
void initializeLogLocation(LogLocation* loc, const std::string& name, Level level)
{
boost::mutex::scoped_lock lock(g_locations_mutex);
if (loc->initialized_)
{
return;
}
loc->logger_ = ::ros::console::impl::getHandle(name);
loc->level_ = level;
g_log_locations.push_back(loc);
checkLogLocationEnabledNoLock(loc);
loc->initialized_ = true;
}
void setLogLocationLevel(LogLocation* loc, Level level)
{
boost::mutex::scoped_lock lock(g_locations_mutex);
loc->level_ = level;
}
void checkLogLocationEnabled(LogLocation* loc)
{
boost::mutex::scoped_lock lock(g_locations_mutex);
checkLogLocationEnabledNoLock(loc);
}
void notifyLoggerLevelsChanged()
{
boost::mutex::scoped_lock lock(g_locations_mutex);
V_LogLocation::iterator it = g_log_locations.begin();
V_LogLocation::iterator end = g_log_locations.end();
for ( ; it != end; ++it )
{
LogLocation* loc = *it;
checkLogLocationEnabledNoLock(loc);
}
}
class StaticInit
{
public:
StaticInit()
{
ROSCONSOLE_AUTOINIT;
}
};
StaticInit g_static_init;
void register_appender(LogAppender* appender)
{
ros::console::impl::register_appender(appender);
}
void shutdown()
{
g_shutting_down = true;
ros::console::impl::shutdown();
}
bool get_loggers(std::map<std::string, levels::Level>& loggers)
{
return ros::console::impl::get_loggers(loggers);
}
bool set_logger_level(const std::string& name, levels::Level level)
{
return ros::console::impl::set_logger_level(name, level);
}
} // namespace console
} // namespace ros

61
thirdparty/ros/ros_comm/tools/rosconsole/src/rosconsole/rosconsole_backend.cpp vendored Normal file
View File

@@ -0,0 +1,61 @@
/*
* Copyright (c) 2013, Open Source Robotics Foundation
* 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 the Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ros/console_backend.h"
namespace ros
{
namespace console
{
namespace backend
{
void notifyLoggerLevelsChanged()
{
if (function_notifyLoggerLevelsChanged)
{
function_notifyLoggerLevelsChanged();
}
}
void (*function_notifyLoggerLevelsChanged)() = 0;
void print(void* logger_handle, ::ros::console::Level level, const char* str, const char* file, const char* function, int line)
{
if (function_print)
{
function_print(logger_handle, level, str, file, function, line);
}
}
void (*function_print)(void*, ::ros::console::Level, const char*, const char*, const char*, int) = 0;
} // namespace backend
} // namespace console
} // namespace ros

76
thirdparty/ros/ros_comm/tools/rosconsole/test/assertion_test.cpp vendored Normal file
View File

@@ -0,0 +1,76 @@
/*
* 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.
*/
#define ROS_ASSERT_ENABLED
#include "ros/assert.h"
#include <gtest/gtest.h>
void doAssert()
{
ROS_ASSERT(false);
}
void doBreak()
{
ROS_BREAK();
}
void doAssertMessage()
{
ROS_ASSERT_MSG(false, "Testing %d %d %d", 1, 2, 3);
}
TEST(RosAssert, assert)
{
ROS_ASSERT(true);
EXPECT_DEATH(doAssert(), "ASSERTION FAILED");
}
TEST(RosAssert, breakpoint)
{
EXPECT_DEATH(doBreak(), "BREAKPOINT HIT");
}
TEST(RosAssert, assertWithMessage)
{
ROS_ASSERT_MSG(true, "Testing %d %d %d", 1, 2, 3);
EXPECT_DEATH(doAssertMessage(), "Testing 1 2 3");
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::Time::init();
testing::FLAGS_gtest_death_test_style = "threadsafe";
return RUN_ALL_TESTS();
}

108
thirdparty/ros/ros_comm/tools/rosconsole/test/thread_test.cpp vendored Normal file
View File

@@ -0,0 +1,108 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived from
* this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "ros/console.h"
#include <gtest/gtest.h>
#include <boost/thread.hpp>
#include "log4cxx/appenderskeleton.h"
#include "log4cxx/spi/loggingevent.h"
#include <vector>
class TestAppender : public log4cxx::AppenderSkeleton
{
public:
struct Info
{
log4cxx::LevelPtr level_;
std::string message_;
std::string logger_name_;
};
typedef std::vector<Info> V_Info;
V_Info info_;
protected:
virtual void append(const log4cxx::spi::LoggingEventPtr& event, log4cxx::helpers::Pool&)
{
Info info;
info.level_ = event->getLevel();
info.message_ = event->getMessage();
info.logger_name_ = event->getLoggerName();
info_.push_back( info );
}
virtual void close()
{
}
virtual bool requiresLayout() const
{
return false;
}
};
void threadFunc(boost::barrier* b)
{
b->wait();
ROS_INFO("Hello");
}
// Ensure all threaded calls go out
TEST(Rosconsole, threadedCalls)
{
log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME);
TestAppender* appender = new TestAppender;
logger->addAppender( appender );
boost::thread_group tg;
boost::barrier b(10);
for (uint32_t i = 0; i < 10; ++i)
{
tg.create_thread(boost::bind(threadFunc, &b));
}
tg.join_all();
ASSERT_EQ(appender->info_.size(), 10ULL);
logger->removeAppender(appender);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
ros::Time::init();
return RUN_ALL_TESTS();
}

1004
thirdparty/ros/ros_comm/tools/rosconsole/test/utest.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

182
thirdparty/ros/ros_comm/tools/rosgraph/CHANGELOG.rst vendored Normal file
View File

@@ -0,0 +1,182 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package rosgraph
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.12.14 (2018-08-23)
--------------------
1.12.13 (2018-02-21)
--------------------
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)
-------------------
* improve message when `roslogging` cannot change permissions (`#1068 <https://github.com/ros/ros_comm/issues/1068>`_)
1.12.7 (2017-02-17)
-------------------
1.12.6 (2016-10-26)
-------------------
* change rospy default rosconsole format for consistency with roscpp (`#879 <https://github.com/ros/ros_comm/pull/879>`_)
* increase request_queue_size for xmlrpc server (`#849 <https://github.com/ros/ros_comm/issues/849>`_)
1.12.5 (2016-09-30)
-------------------
1.12.4 (2016-09-19)
-------------------
1.12.3 (2016-09-17)
-------------------
* add 'Darwin' to unix-like platforms improving address resolution (`#846 <https://github.com/ros/ros_comm/pull/846>`_)
* use logging Formatter, enabling printing exception info with exc_info=1 (`#828 <https://github.com/ros/ros_comm/pull/828>`_)
* add `__contains_\_`, which is a better spelling of `has` (`#754 <https://github.com/ros/ros_comm/pull/754>`_)
1.12.2 (2016-06-03)
-------------------
* avoid creating a latest symlink for the root of the log dir (`#795 <https://github.com/ros/ros_comm/pull/795>`_)
1.12.1 (2016-04-18)
-------------------
* fix str conversion in encode_ros_handshake_header (`#792 <https://github.com/ros/ros_comm/pull/792>`_)
1.12.0 (2016-03-18)
-------------------
1.11.18 (2016-03-17)
--------------------
1.11.17 (2016-03-11)
--------------------
* fix raising classes not derived from `Exception` which caused a TypeError (`#761 <https://github.com/ros/ros_comm/pull/761>`_)
* fix handshake header with Python 3 (`#759 <https://github.com/ros/ros_comm/issues/759>`_)
* fix encoding of header fields (`#704 <https://github.com/ros/ros_comm/issues/704>`_)
1.11.16 (2015-11-09)
--------------------
1.11.15 (2015-10-13)
--------------------
1.11.14 (2015-09-19)
--------------------
* create a symlink to the latest log directory (`#659 <https://github.com/ros/ros_comm/pull/659>`_)
1.11.13 (2015-04-28)
--------------------
1.11.12 (2015-04-27)
--------------------
1.11.11 (2015-04-16)
--------------------
1.11.10 (2014-12-22)
--------------------
* rosconsole format for rospy (`#517 <https://github.com/ros/ros_comm/issues/517>`_)
* fix exception at roscore startup if python has IPv6 disabled (`#515 <https://github.com/ros/ros_comm/issues/515>`_)
1.11.9 (2014-08-18)
-------------------
1.11.8 (2014-08-04)
-------------------
1.11.7 (2014-07-18)
-------------------
1.11.6 (2014-07-10)
-------------------
1.11.5 (2014-06-24)
-------------------
1.11.4 (2014-06-16)
-------------------
* Python 3 compatibility (`#426 <https://github.com/ros/ros_comm/issues/426>`_, `#427 <https://github.com/ros/ros_comm/issues/427>`_, `#429 <https://github.com/ros/ros_comm/issues/429>`_)
1.11.3 (2014-05-21)
-------------------
1.11.2 (2014-05-08)
-------------------
1.11.1 (2014-05-07)
-------------------
* add architecture_independent flag in package.xml (`#391 <https://github.com/ros/ros_comm/issues/391>`_)
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)
-------------------
1.9.54 (2014-01-27)
-------------------
1.9.53 (2014-01-14)
-------------------
1.9.52 (2014-01-08)
-------------------
1.9.51 (2014-01-07)
-------------------
* allow different 127. addresses than 127.0.0.1 (`#315 <https://github.com/ros/ros_comm/issues/315>`_)
* work around for nose 1.3.0 bug (`#323 <https://github.com/ros/ros_comm/issues/323>`_)
1.9.50 (2013-10-04)
-------------------
1.9.49 (2013-09-16)
-------------------
1.9.48 (2013-08-21)
-------------------
1.9.47 (2013-07-03)
-------------------
* check for CATKIN_ENABLE_TESTING to enable configure without tests
1.9.46 (2013-06-18)
-------------------
1.9.45 (2013-06-06)
-------------------
* add warnings for obviously wrong environment variables ROS_HOSTNAME and ROS_IP (`#134 <https://github.com/ros/ros_comm/issues/134>`_)
* fix exception on netifaces.ifaddresses() (`#211 <https://github.com/ros/ros_comm/issues/211>`_, `#213 <https://github.com/ros/ros_comm/issues/213>`_) (regression from 1.9.42)
1.9.44 (2013-03-21)
-------------------
1.9.43 (2013-03-13)
-------------------
1.9.42 (2013-03-08)
-------------------
* replace custom code with Python module netifaces (`#130 <https://github.com/ros/ros_comm/issues/130>`_)
* make dependencies on rospy optional by refactoring RosStreamHandler to rosgraph (`#179 <https://github.com/ros/ros_comm/issues/179>`_)
1.9.41 (2013-01-24)
-------------------
1.9.40 (2013-01-13)
-------------------
* add colorization for rospy log output (`#3691 <https://code.ros.org/trac/ros/ticket/3691>`_)
1.9.39 (2012-12-29)
-------------------
* first public release for Groovy

20
thirdparty/ros/ros_comm/tools/rosgraph/CMakeLists.txt vendored Normal file
View File

@@ -0,0 +1,20 @@
cmake_minimum_required(VERSION 2.8.3)
project(rosgraph)
find_package(catkin REQUIRED)
catkin_package()
catkin_python_setup()
# logging config file goes in both etc and package layout (for now).
# want to get rid of package layout copy, but need to be able to
# locate etc first.
install(FILES conf/python_logging.conf
DESTINATION ${CATKIN_GLOBAL_ETC_DESTINATION}/ros)
install(FILES conf/python_logging.conf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/conf)
catkin_install_python(PROGRAMS scripts/rosgraph
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
if(CATKIN_ENABLE_TESTING)
catkin_add_nosetests(test)
endif()

35
thirdparty/ros/ros_comm/tools/rosgraph/conf/python_logging.conf vendored Normal file
View File

@@ -0,0 +1,35 @@
[loggers]
keys=root, rosout
[handlers]
keys=fileHandler,streamHandler
[formatters]
keys=defaultFormatter
[logger_root]
level=INFO
handlers=fileHandler
[logger_rosout]
level=INFO
handlers=streamHandler
propagate=1
qualname=rosout
[handler_fileHandler]
class=handlers.RotatingFileHandler
level=DEBUG
formatter=defaultFormatter
# log filename, mode, maxBytes, backupCount
args=(os.environ['ROS_LOG_FILENAME'],'a', 50000000, 4)
[handler_streamHandler]
class=rosgraph.roslogging.RosStreamHandler
level=DEBUG
formatter=defaultFormatter
# colorize output flag
args=(True,)
[formatter_defaultFormatter]
format=[%(name)s][%(levelname)s] %(asctime)s: %(message)s

26
thirdparty/ros/ros_comm/tools/rosgraph/package.xml vendored Normal file
View File

@@ -0,0 +1,26 @@
<package>
<name>rosgraph</name>
<version>1.12.14</version>
<description>
rosgraph contains the rosgraph command-line tool, which prints
information about the ROS Computation Graph. It also provides an
internal library that can be used by graphical tools.
</description>
<maintainer email="dthomas@osrfoundation.org">Dirk Thomas</maintainer>
<license>BSD</license>
<url>http://ros.org/wiki/rosgraph</url>
<author>Ken Conley</author>
<buildtool_depend version_gte="0.5.78">catkin</buildtool_depend>
<run_depend>python-netifaces</run_depend>
<run_depend>python-rospkg</run_depend>
<test_depend>python-mock</test_depend>
<export>
<rosdoc config="rosdoc.yaml"/>
<architecture_independent/>
</export>
</package>

1
thirdparty/ros/ros_comm/tools/rosgraph/rosdoc.yaml vendored Normal file
View File

@@ -0,0 +1 @@
- builder: epydoc

38
thirdparty/ros/ros_comm/tools/rosgraph/scripts/rosgraph vendored Executable file
View File

@@ -0,0 +1,38 @@
#!/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.
#
# Revision $Id: rosgraph 3804 2009-02-11 02:16:00Z rob_wheeler $
import rosgraph.rosgraph_main
rosgraph.rosgraph_main.rosgraph_main()

13
thirdparty/ros/ros_comm/tools/rosgraph/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=['rosgraph', 'rosgraph.impl'],
package_dir={'': 'src'},
scripts=['scripts/rosgraph'],
requires=['rospkg']
)
setup(**d)

56
thirdparty/ros/ros_comm/tools/rosgraph/src/rosgraph/__init__.py vendored Normal file
View File

@@ -0,0 +1,56 @@
# 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
from . rosenv import get_master_uri, ROS_MASTER_URI, ROS_NAMESPACE, ROS_HOSTNAME, ROS_IP, ROS_IPV6
from . masterapi import Master, MasterFailure, MasterError, MasterException
from . masterapi import is_online as is_master_online
# bring in names submodule
from . import names
def myargv(argv=None):
"""
Remove ROS remapping arguments from sys.argv arguments.
:returns: copy of sys.argv with ROS remapping arguments removed, ``[str]``
"""
if argv is None:
argv = sys.argv
return [a for a in argv if not names.REMAP in a]
__all__ = ['myargv',
'get_master_uri', 'ROS_MASTER_URI', 'ROS_NAMESPACE', 'ROS_HOSTNAME', 'ROS_IP', 'ROS_IPV6',
'Master', 'MasterFailure', 'MasterError', 'MasterException',
'is_master_online']

34
thirdparty/ros/ros_comm/tools/rosgraph/src/rosgraph/impl/__init__.py vendored Normal file
View File

@@ -0,0 +1,34 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: __init__.py 5735 2009-08-20 21:31:27Z sfkwc $

582
thirdparty/ros/ros_comm/tools/rosgraph/src/rosgraph/impl/graph.py vendored Normal file
View File

@@ -0,0 +1,582 @@
# 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.
#
# Revision $Id$
from __future__ import print_function
"""
Data structures and library for representing ROS Computation Graph state.
"""
import sys
import time
import itertools
import random
import logging
import traceback
try:
from xmlrpc.client import ServerProxy
except ImportError:
from xmlrpclib import ServerProxy
import socket
import rosgraph.masterapi
logger = logging.getLogger('rosgraph.graph')
_ROS_NAME = '/rosviz'
def topic_node(topic):
"""
In order to prevent topic/node name aliasing, we have to remap
topic node names. Currently we just prepend a space, which is
an illegal ROS name and thus not aliased.
@return str: topic mapped to a graph node name.
"""
return ' ' + topic
def node_topic(node):
"""
Inverse of topic_node
@return str: undo topic_node() operation
"""
return node[1:]
class BadNode(object):
"""
Data structure for storing info about a 'bad' node
"""
## no connectivity
DEAD = 0
## intermittent connectivity
WONKY = 1
def __init__(self, name, type, reason):
"""
@param type: DEAD | WONKY
@type type: int
"""
self.name =name
self.reason = reason
self.type = type
class EdgeList(object):
"""
Data structure for storing Edge instances
"""
__slots__ = ['edges_by_start', 'edges_by_end']
def __init__(self):
# in order to make it easy to purge edges, we double-index them
self.edges_by_start = {}
self.edges_by_end = {}
def __iter__(self):
return itertools.chain(*[v for v in self.edges_by_start.values()])
def has(self, edge):
return edge in self
def __contains__(self, edge):
"""
@return: True if edge is in edge list
@rtype: bool
"""
key = edge.key
return key in self.edges_by_start and \
edge in self.edges_by_start[key]
def add(self, edge):
"""
Add an edge to our internal representation. not multi-thread safe
@param edge: edge to add
@type edge: Edge
"""
# see note in __init__
def update_map(map, key, edge):
if key in map:
l = map[key]
if not edge in l:
l.append(edge)
return True
else:
return False
else:
map[key] = [edge]
return True
updated = update_map(self.edges_by_start, edge.key, edge)
updated = update_map(self.edges_by_end, edge.rkey, edge) or updated
return updated
def add_edges(self, start, dest, direction, label=''):
"""
Create Edge instances for args and add resulting edges to edge
list. Convenience method to avoid repetitve logging, etc...
@param edge_list: data structure to add edge to
@type edge_list: EdgeList
@param start: name of start node. If None, warning will be logged and add fails
@type start: str
@param dest: name of start node. If None, warning will be logged and add fails
@type dest: str
@param direction: direction string (i/o/b)
@type direction: str
@return: True if update occured
@rtype: bool
"""
# the warnings should generally be temporary, occuring of the
# master/node information becomes stale while we are still
# doing an update
updated = False
if not start:
logger.warn("cannot add edge: cannot map start [%s] to a node name", start)
elif not dest:
logger.warn("cannot add edge: cannot map dest [%s] to a node name", dest)
else:
for args in edge_args(start, dest, direction, label):
updated = self.add(Edge(*args)) or updated
return updated
def delete_all(self, node):
"""
Delete all edges that start or end at node
@param node: name of node
@type node: str
"""
def matching(map, pref):
return [map[k] for k in map.keys() if k.startswith(pref)]
pref = node+"|"
edge_lists = matching(self.edges_by_start, pref) + matching(self.edges_by_end, pref)
for el in edge_lists:
for e in el:
self.delete(e)
def delete(self, edge):
# see note in __init__
def update_map(map, key, edge):
if key in map:
edges = map[key]
if edge in edges:
edges.remove(edge)
return True
update_map(self.edges_by_start, edge.key, edge)
update_map(self.edges_by_end, edge.rkey, edge)
class Edge(object):
"""
Data structure for representing ROS node graph edge
"""
__slots__ = ['start', 'end', 'label', 'key', 'rkey']
def __init__(self, start, end, label=''):
self.start = start
self.end = end
self.label = label
self.key = "%s|%s"%(self.start, self.label)
# reverse key, indexed from end
self.rkey = "%s|%s"%(self.end, self.label)
def __ne__(self, other):
return self.start != other.start or self.end != other.end
def __str__(self):
return "%s->%s"%(self.start, self.end)
def __eq__(self, other):
return self.start == other.start and self.end == other.end
def edge_args(start, dest, direction, label):
"""
compute argument ordering for Edge constructor based on direction flag
@param direction str: 'i', 'o', or 'b' (in/out/bidir) relative to \a start
@param start str: name of starting node
@param start dest: name of destination node
"""
edge_args = []
if direction in ['o', 'b']:
edge_args.append((start, dest, label))
if direction in ['i', 'b']:
edge_args.append((dest, start, label))
return edge_args
class Graph(object):
"""
Utility class for polling ROS statistics from running ROS graph.
Not multi-thread-safe
"""
def __init__(self, node_ns='/', topic_ns='/'):
self.master = rosgraph.masterapi.Master(_ROS_NAME)
self.node_ns = node_ns or '/'
self.topic_ns = topic_ns or '/'
# ROS nodes
self.nn_nodes = set([])
# ROS topic nodes
self.nt_nodes = set([])
# ROS nodes that aren't responding quickly
self.bad_nodes = {}
import threading
self.bad_nodes_lock = threading.Lock()
# ROS services
self.srvs = set([])
# ROS node->node transport connections
self.nn_edges = EdgeList()
# ROS node->topic connections
self.nt_edges = EdgeList()
# ROS all node->topic connections, including empty
self.nt_all_edges = EdgeList()
# node names to URI map
self.node_uri_map = {} # { node_name_str : uri_str }
# reverse map URIs to node names
self.uri_node_map = {} # { uri_str : node_name_str }
# time we last contacted master
self.last_master_refresh = 0
self.last_node_refresh = {}
# time we last communicated with master
# seconds until master data is considered stale
self.master_stale = 5.0
# time we last communicated with node
# seconds until node data is considered stale
self.node_stale = 5.0 #seconds
def set_master_stale(self, stale_secs):
"""
@param stale_secs: seconds that data is considered fresh
@type stale_secs: double
"""
self.master_stale = stale_secs
def set_node_stale(self, stale_secs):
"""
@param stale_secs: seconds that data is considered fresh
@type stale_secs: double
"""
self.node_stale = stale_secs
def _master_refresh(self):
"""
@return: True if nodes information was updated
@rtype: bool
"""
logger.debug("master refresh: starting")
updated = False
try:
val = self.master.getSystemState()
except rosgraph.masterapi.MasterException as e:
print("Unable to contact master", str(e), file=sys.stderr)
logger.error("unable to contact master: %s", str(e))
return False
pubs, subs, srvs = val
nodes = []
nt_all_edges = self.nt_all_edges
nt_nodes = self.nt_nodes
for state, direction in ((pubs, 'o'), (subs, 'i')):
for topic, l in state:
if topic.startswith(self.topic_ns):
nodes.extend([n for n in l if n.startswith(self.node_ns)])
nt_nodes.add(topic_node(topic))
for node in l:
updated = nt_all_edges.add_edges(
node, topic_node(topic), direction) or updated
nodes = set(nodes)
srvs = set([s for s, _ in srvs])
purge = None
if nodes ^ self.nn_nodes:
purge = self.nn_nodes - nodes
self.nn_nodes = nodes
updated = True
if srvs ^ self.srvs:
self.srvs = srvs
updated = True
if purge:
logger.debug("following nodes and related edges will be purged: %s", ','.join(purge))
for p in purge:
logger.debug('purging edges for node %s', p)
self.nn_edges.delete_all(p)
self.nt_edges.delete_all(p)
self.nt_all_edges.delete_all(p)
logger.debug("master refresh: done, updated[%s]", updated)
return updated
def _mark_bad_node(self, node, reason):
try:
# bad nodes are updated in a separate thread, so lock
self.bad_nodes_lock.acquire()
if node in self.bad_nodes:
self.bad_nodes[node].type = BadNode.DEAD
else:
self.bad_nodes[node] = (BadNode(node, BadNode.DEAD, reason))
finally:
self.bad_nodes_lock.release()
def _unmark_bad_node(self, node, reason):
"""
Promotes bad node to 'wonky' status.
"""
try:
# bad nodes are updated in a separate thread, so lock
self.bad_nodes_lock.acquire()
bad = self.bad_nodes[node]
bad.type = BadNode.WONKY
finally:
self.bad_nodes_lock.release()
def _node_refresh_businfo(self, node, api, bad_node=False):
"""
Retrieve bus info from the node and update nodes and edges as appropriate
@param node: node name
@type node: str
@param api: XML-RPC proxy
@type api: ServerProxy
@param bad_node: If True, node has connectivity issues and
should be treated differently
@type bad_node: bool
"""
try:
logger.debug("businfo: contacting node [%s] for bus info", node)
# unmark bad node, though it stays on the bad list
if bad_node:
self._unmark_bad_node(node)
# Lower the socket timeout as we cannot abide by slow HTTP timeouts.
# If a node cannot meet this timeout, it goes on the bad list
# TODO: override transport instead.
old_timeout = socket.getdefaulttimeout()
if bad_node:
#even stricter timeout for bad_nodes right now
socket.setdefaulttimeout(0.2)
else:
socket.setdefaulttimeout(1.0)
code, msg, bus_info = api.getBusInfo(_ROS_NAME)
socket.setdefaulttimeout(old_timeout)
except Exception as e:
# node is (still) bad
self._mark_bad_node(node, str(e))
code = -1
msg = traceback.format_exc()
updated = False
if code != 1:
logger.error("cannot get stats info from node [%s]: %s", node, msg)
else:
# [[connectionId1, destinationId1, direction1, transport1, ...]... ]
for info in bus_info:
# #3579 bad node, ignore
if len(info) < 5:
continue
connection_id = info[0]
dest_id = info[1]
direction = info[2]
transport = info[3]
topic = info[4]
if len(info) > 5:
connected = info[5]
else:
connected = True #backwards compatibility
if connected and topic.startswith(self.topic_ns):
# blindly add as we will be able to catch state change via edges.
# this currently means we don't cleanup topics
self.nt_nodes.add(topic_node(topic))
# update node->topic->node graph edges
updated = self.nt_edges.add_edges(node, topic_node(topic), direction) or updated
# update node->node graph edges
if dest_id.startswith('http://'):
#print("FOUND URI", dest_id)
dest_name = self.uri_node_map.get(dest_id, None)
updated = self.nn_edges.add_edges(node, dest_name, direction, topic) or updated
else:
#TODO: anyting to do here?
pass
return updated
def _node_refresh(self, node, bad_node=False):
"""
Contact node for stats/connectivity information
@param node: name of node to contact
@type node: str
@param bad_node: if True, node has connectivity issues
@type bad_node: bool
@return: True if node was successfully contacted
@rtype bool
"""
# TODO: I'd like for master to provide this information in
# getSystemState() instead to prevent the extra connection per node
updated = False
uri = self._node_uri_refresh(node)
try:
if uri:
api = ServerProxy(uri)
updated = self._node_refresh_businfo(node, api, bad_node)
except KeyError as e:
logger.warn('cannot contact node [%s] as it is not in the lookup table'%node)
return updated
def _node_uri_refresh(self, node):
try:
uri = self.master.lookupNode(node)
except:
msg = traceback.format_exc()
logger.warn("master reported error in node lookup: %s"%msg)
return None
# update maps
self.node_uri_map[node] = uri
self.uri_node_map[uri] = node
return uri
def _node_uri_refresh_all(self):
"""
Build self.node_uri_map and self.uri_node_map using master as a
lookup service. This will make N requests to the master for N
nodes, so this should only be used sparingly
"""
for node in self.nn_nodes:
self._node_uri_refresh(node)
def bad_update(self):
"""
Update loop for nodes with bad connectivity. We box them separately
so that we can maintain the good performance of the normal update loop.
Once a node is on the bad list it stays there.
"""
last_node_refresh = self.last_node_refresh
# nodes left to check
try:
self.bad_nodes_lock.acquire()
# make copy due to multithreading
update_queue = self.bad_nodes.values()[:]
finally:
self.bad_nodes_lock.release()
# return value. True if new data differs from old
updated = False
# number of nodes we checked
num_nodes = 0
start_time = time.time()
while update_queue:
# figure out the next node to contact
next = update_queue.pop()
# rate limit talking to any particular node
if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale):
updated = self._node_refresh(next.name, True) or updated
# include in random offset (max 1/5th normal update interval)
# to help spread out updates
last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0)
num_nodes += 1
# small yield to keep from torquing the processor
time.sleep(0.01)
end_time = time.time()
#print("Update (bad nodes) took %ss for %s nodes"%((end_time-start_time), num_nodes))
logger.debug("ROS stats (bad nodes) update took %ss"%(end_time-start_time))
return updated
def update(self):
"""
Update all the stats. This method may take awhile to complete as it will
communicate with all nodes + master.
"""
last_node_refresh = self.last_node_refresh
# nodes left to check
update_queue = None
# True if there are still more stats to fetch this cycle
work_to_do = True
# return value. True if new data differs from old
updated = False
# number of nodes we checked
num_nodes = 0
start_time = time.time()
while work_to_do:
# each time through the loop try to talk to either the master
# or a node. stop when we have talked to everybody.
# get a new node list from the master
if time.time() > (self.last_master_refresh + self.master_stale):
updated = self._master_refresh()
if self.last_master_refresh == 0:
# first time we contact the master, also refresh our full lookup tables
self._node_uri_refresh_all()
self.last_master_refresh = time.time()
# contact the nodes for stats
else:
# initialize update_queue based on most current nodes list
if update_queue is None:
update_queue = list(self.nn_nodes)
# no nodes left to contact
elif not update_queue:
work_to_do = False
# contact next node
else:
# figure out the next node to contact
next = update_queue.pop()
# rate limit talking to any particular node
if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale):
updated = self._node_refresh(next) or updated
# include in random offset (max 1/5th normal update interval)
# to help spread out updates
last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0)
num_nodes += 1
# small yield to keep from torquing the processor
time.sleep(0.01)
end_time = time.time()
#print("Update took %ss for %s nodes"%((end_time-start_time), num_nodes))
logger.debug("ROS stats update took %ss"%(end_time-start_time))
return updated

481
thirdparty/ros/ros_comm/tools/rosgraph/src/rosgraph/masterapi.py vendored Normal file
View File

@@ -0,0 +1,481 @@
# Software License Agreement (BSD License)
#
# Copyright (c) 2009, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# * Redistributions in binary form must reproduce the above
# copyright notice, this list of conditions and the following
# disclaimer in the documentation and/or other materials provided
# with the distribution.
# * Neither the name of Willow Garage, Inc. nor the names of its
# contributors may be used to endorse or promote products derived
# from this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Revision $Id: masterapi.py 9672 2010-05-11 21:57:40Z kwc $
"""
Python adapter for calling ROS Master API. While it is trivial to call the
Master directly using XML-RPC, this API provides a safer abstraction in the event
the Master API is changed.
"""
try:
from xmlrpc.client import ServerProxy # Python 3.x
except ImportError:
from xmlrpclib import ServerProxy # Python 2.x
from . names import make_caller_id
from . rosenv import get_master_uri
from . network import parse_http_host_and_port
class MasterException(Exception):
"""
Base class of ROS-master related errors.
"""
pass
class MasterFailure(MasterException):
"""
Call to Master failed. This generally indicates an internal error
in the Master and that the Master may be in an inconsistent state.
"""
pass
class MasterError(MasterException):
"""
Master returned an error code, which indicates an error in the
arguments passed to the Master.
"""
pass
# backwards compat
ROSMasterException = MasterException
Error = MasterError
Failure = MasterFailure
def is_online(master_uri=None):
"""
@param master_uri: (optional) override environment's ROS_MASTER_URI
@type master_uri: str
@return: True if Master is available
"""
return Master('rosgraph', master_uri=master_uri).is_online()
class Master(object):
"""
API for interacting with the ROS master. Although the Master is
relatively simple to interact with using the XMLRPC API, this
abstraction layer provides protection against future updates. It
also provides a streamlined API with builtin return code checking
and caller_id passing.
"""
def __init__(self, caller_id, master_uri=None):
"""
:param caller_id: name of node to use in calls to master, ``str``
:param master_uri: (optional) override default ROS master URI, ``str``
:raises: :exc:`ValueError` If ROS master uri not set properly
"""
if master_uri is None:
master_uri = get_master_uri()
self._reinit(master_uri)
self.caller_id = make_caller_id(caller_id) #resolve
if self.caller_id[-1] == '/':
self.caller_id = self.caller_id[:-1]
def _reinit(self, master_uri):
"""
Internal API for reinitializing this handle to be a new master
:raises: :exc:`ValueError` If ROS master uri not set
"""
if master_uri is None:
raise ValueError("ROS master URI is not set")
# #1730 validate URL for better error messages
try:
parse_http_host_and_port(master_uri)
except ValueError:
raise ValueError("invalid master URI: %s"%(master_uri))
self.master_uri = master_uri
self.handle = ServerProxy(self.master_uri)
def is_online(self):
"""
Check if Master is online.
NOTE: this is not part of the actual Master API. This is a convenience function.
@param master_uri: (optional) override environment's ROS_MASTER_URI
@type master_uri: str
@return: True if Master is available
"""
try:
self.getPid()
return True
except:
return False
def _succeed(self, args):
"""
Check master return code and return the value field.
@param args: master return value
@type args: (int, str, XMLRPCLegalValue)
@return: value field of args (master return value)
@rtype: XMLRPCLegalValue
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
code, msg, val = args
if code == 1:
return val
elif code == -1:
raise Error(msg)
else:
raise Failure(msg)
################################################################################
# PARAM SERVER
def deleteParam(self, key):
"""
Parameter Server: delete parameter
@param key: parameter name
@type key: str
@return: 0
@rtype: int
"""
return self._succeed(self.handle.deleteParam(self.caller_id, key))
def setParam(self, key, value):
"""
Parameter Server: set parameter. NOTE: if value is a
dictionary it will be treated as a parameter tree, where key
is the parameter namespace. For example:::
{'x':1,'y':2,'sub':{'z':3}}
will set key/x=1, key/y=2, and key/sub/z=3. Furthermore, it
will replace all existing parameters in the key parameter
namespace with the parameters in value. You must set
parameters individually if you wish to perform a union update.
@param key: parameter name
@type key: str
@param value: parameter value.
@type value: XMLRPCLegalValue
@return: 0
@rtype: int
"""
return self._succeed(self.handle.setParam(self.caller_id, key, value))
def getParam(self, key):
"""
Retrieve parameter value from server.
@param key: parameter to lookup. If key is a namespace,
getParam() will return a parameter tree.
@type key: str
getParam() will return a parameter tree.
@return: parameterValue. If key is a namespace,
the return value will be a dictionary, where each key is a
parameter in that namespace. Sub-namespaces are also
represented as dictionaries.
@rtype: XMLRPCLegalValue
"""
return self._succeed(self.handle.getParam(self.caller_id, key))
def searchParam(self, key):
"""
Search for parameter key on parameter server. Search starts in caller's namespace and proceeds
upwards through parent namespaces until Parameter Server finds a matching key.
searchParam's behavior is to search for the first partial match.
For example, imagine that there are two 'robot_description' parameters::
/robot_description
/robot_description/arm
/robot_description/base
/pr2/robot_description
/pr2/robot_description/base
If I start in the namespace /pr2/foo and search for
'robot_description', searchParam will match
/pr2/robot_description. If I search for 'robot_description/arm'
it will return /pr2/robot_description/arm, even though that
parameter does not exist (yet).
@param key: parameter key to search for.
@type key: str
@return: foundKey
@rtype: str
"""
return self._succeed(self.handle.searchParam(self.caller_id, key))
def subscribeParam(self, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: parameterValue. parameterValue is an empty dictionary if the parameter has not been set yet.
@rtype: XMLRPCLegalValue
"""
return self._succeed(self.handle.subscribeParam(self.caller_id, caller_api, key))
def unsubscribeParam(self, caller_api, key):
"""
Retrieve parameter value from server and subscribe to updates to that param. See
paramUpdate() in the Node API.
@param key: parameter to lookup.
@type key: str
@param caller_api: API URI for paramUpdate callbacks.
@type caller_api: str
@return: numUnsubscribed. If numUnsubscribed is zero it means that the caller was not subscribed to the parameter.
@rtype: int
"""
return self._succeed(self.handle.unsubscribeParam(self.caller_id, caller_api, key))
def hasParam(self, key):
"""
Check if parameter is stored on server.
@param key: parameter to check
@type key: str
@return: [code, statusMessage, hasParam]
@rtype: [int, str, bool]
"""
return self._succeed(self.handle.hasParam(self.caller_id, key))
def getParamNames(self):
"""
Get list of all parameter names stored on this server.
This does not adjust parameter names for caller's scope.
@return: [code, statusMessage, parameterNameList]
@rtype: [int, str, [str]]
"""
return self._succeed(self.handle.getParamNames(self.caller_id))
################################################################################
def getPid(self):
"""
Get the PID of this server
@return: serverProcessPID
@rtype: int
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getPid(self.caller_id))
def getUri(self):
"""
Get the URI of this Master
@return: masterUri
@rtype: str
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getUri(self.caller_id))
def registerService(self, service, service_api, caller_api):
"""
Register the caller as a provider of the specified service.
@param service str: Fully-qualified name of service
@param service_api str: Service URI
@param caller_api str: XML-RPC URI of caller node
@return: ignore
@rtype: int
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.registerService(self.caller_id, service, service_api, caller_api))
def lookupService(self, service):
"""
Lookup all provider of a particular service.
@param service: fully-qualified name of service to lookup.
@type: service: str
@return (int, str, str): (code, message, serviceUrl). service URL is provides
and address and port of the service. Fails if there is no provider.
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.lookupService(self.caller_id, service))
def unregisterService(self, service, service_api):
"""
Unregister the caller as a provider of the specified service.
@param service: Fully-qualified name of service
@type service: str
@param service_api: API URI of service to unregister. Unregistration will only occur if current
registration matches.
@type service_api: str
@return: (code, message, numUnregistered). Number of unregistrations (either 0 or 1).
If this is zero it means that the caller was not registered as a service provider.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.unregisterService(self.caller_id, service, service_api))
def registerSubscriber(self, topic, topic_type, caller_api):
"""
Subscribe the caller to the specified topic. In addition to receiving
a list of current publishers, the subscriber will also receive notifications
of new publishers via the publisherUpdate API.
@param topic str: Fully-qualified name of topic to subscribe to.
@param topic_type: Datatype for topic. Must be a package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api: XML-RPC URI of caller node for new publisher notifications
@type caller_api: str
@return: (code, message, publishers). Publishers is a list of XMLRPC API URIs
for nodes currently publishing the specified topic.
@rtype: (int, str, list(str))
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.registerSubscriber(self.caller_id, topic, topic_type, caller_api))
def unregisterSubscriber(self, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api: API URI of service to unregister. Unregistration will only occur if current
@type caller_api: str
registration matches.
@return: (code, statusMessage, numUnsubscribed).
If numUnsubscribed is zero it means that the caller was not registered as a subscriber.
The call still succeeds as the intended final state is reached.
@rtype: (int, str, int)
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.unregisterSubscriber(self.caller_id, topic, caller_api))
def registerPublisher(self, topic, topic_type, caller_api):
"""
Register the caller as a publisher the topic.
@param topic: Fully-qualified name of topic to register.
@type topic: str
@param topic_type: Datatype for topic. Must be a
package-resource name, i.e. the .msg name.
@type topic_type: str
@param caller_api str: ROS caller XML-RPC API URI
@type caller_api: str
@return: subscriberApis.
List of current subscribers of topic in the form of XMLRPC URIs.
@rtype: [str]
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.registerPublisher(self.caller_id, topic, topic_type, caller_api))
def unregisterPublisher(self, topic, caller_api):
"""
Unregister the caller as a publisher of the topic.
@param topic: Fully-qualified name of topic to unregister.
@type topic: str
@param caller_api str: API URI of service to
unregister. Unregistration will only occur if current
registration matches.
@type caller_api: str
@return: numUnregistered.
If numUnregistered is zero it means that the caller was not registered as a publisher.
The call still succeeds as the intended final state is reached.
@rtype: int
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.unregisterPublisher(self.caller_id, topic, caller_api))
def lookupNode(self, node_name):
"""
Get the XML-RPC URI of the node with the associated
name/caller_id. This API is for looking information about
publishers and subscribers. Use lookupService instead to lookup
ROS-RPC URIs.
@param node: name of node to lookup
@type node: str
@return: URI
@rtype: str
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.lookupNode(self.caller_id, node_name))
def getPublishedTopics(self, subgraph):
"""
Get list of topics that can be subscribed to. This does not return topics that have no publishers.
See L{getSystemState()} to get more comprehensive list.
@param subgraph: Restrict topic names to match within the specified subgraph. Subgraph namespace
is resolved relative to the caller's namespace. Use '' to specify all names.
@type subgraph: str
@return: [[topic1, type1]...[topicN, typeN]]
@rtype: [[str, str],]
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getPublishedTopics(self.caller_id, subgraph))
def getTopicTypes(self):
"""
Retrieve list topic names and their types.
New in ROS 1.2.
@rtype: (int, str, [[str,str]] )
@return: (code, statusMessage, topicTypes). topicTypes is a list of [topicName, topicType] pairs.
"""
return self._succeed(self.handle.getTopicTypes(self.caller_id))
def getSystemState(self):
"""
Retrieve list representation of system state (i.e. publishers, subscribers, and services).
@rtype: [[str,[str]], [str,[str]], [str,[str]]]
@return: systemState
System state is in list representation::
[publishers, subscribers, services].
publishers is of the form::
[ [topic1, [topic1Publisher1...topic1PublisherN]] ... ]
subscribers is of the form::
[ [topic1, [topic1Subscriber1...topic1SubscriberN]] ... ]
services is of the form::
[ [service1, [service1Provider1...service1ProviderN]] ... ]
@raise rosgraph.masterapi.Error: if Master returns ERROR.
@raise rosgraph.masterapi.Failure: if Master returns FAILURE.
"""
return self._succeed(self.handle.getSystemState(self.caller_id))

329
thirdparty/ros/ros_comm/tools/rosgraph/src/rosgraph/names.py vendored Normal file
View File

@@ -0,0 +1,329 @@
# 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.
#
# Revision $Id: names.py 14589 2011-08-07 18:30:21Z kwc $
"""
Library for manipulating ROS Names. See U{http://ros.org/wiki/Names}.
"""
import os
import sys
from .rosenv import ROS_NAMESPACE
#TODO: why are these here?
MSG_EXT = '.msg'
SRV_EXT = '.srv'
SEP = '/'
GLOBALNS = '/'
PRIV_NAME = '~'
REMAP = ":="
ANYTYPE = '*'
if sys.hexversion > 0x03000000: #Python3
def isstring(s):
return isinstance(s, str) #Python 3.x
else:
def isstring(s):
"""
Small helper version to check an object is a string in a way that works
for both Python 2 and 3
"""
return isinstance(s, basestring) #Python 2.x
def get_ros_namespace(env=None, argv=None):
"""
@param env: environment dictionary (defaults to os.environ)
@type env: dict
@param argv: command-line arguments (defaults to sys.argv)
@type argv: [str]
@return: ROS namespace of current program
@rtype: str
"""
#we force command-line-specified namespaces to be globally scoped
if argv is None:
argv = sys.argv
for a in argv:
if a.startswith('__ns:='):
return make_global_ns(a[len('__ns:='):])
if env is None:
env = os.environ
return make_global_ns(env.get(ROS_NAMESPACE, GLOBALNS))
def make_caller_id(name):
"""
Resolve a local name to the caller ID based on ROS environment settings (i.e. ROS_NAMESPACE)
@param name: local name to calculate caller ID from, e.g. 'camera', 'node'
@type name: str
@return: caller ID based on supplied local name
@rtype: str
"""
return make_global_ns(ns_join(get_ros_namespace(), name))
def make_global_ns(name):
"""
Convert name to a global name with a trailing namespace separator.
@param name: ROS resource name. Cannot be a ~name.
@type name: str
@return str: name as a global name, e.g. 'foo' -> '/foo/'.
This does NOT resolve a name.
@rtype: str
@raise ValueError: if name is a ~name
"""
if is_private(name):
raise ValueError("cannot turn [%s] into a global name"%name)
if not is_global(name):
name = SEP + name
if name[-1] != SEP:
name = name + SEP
return name
def is_global(name):
"""
Test if name is a global graph resource name.
@param name: must be a legal name in canonical form
@type name: str
@return: True if name is a globally referenced name (i.e. /ns/name)
@rtype: bool
"""
return name and name[0] == SEP
def is_private(name):
"""
Test if name is a private graph resource name.
@param name: must be a legal name in canonical form
@type name: str
@return bool: True if name is a privately referenced name (i.e. ~name)
"""
return name and name[0] == PRIV_NAME
def namespace(name):
"""
Get the namespace of name. The namespace is returned with a
trailing slash in order to favor easy concatenation and easier use
within the global context.
@param name: name to return the namespace of. Must be a legal
name. NOTE: an empty name will return the global namespace.
@type name: str
@return str: Namespace of name. For example, '/wg/node1' returns '/wg/'. The
global namespace is '/'.
@rtype: str
@raise ValueError: if name is invalid
"""
"map name to its namespace"
if name is None:
raise ValueError('name')
if not isstring(name):
raise TypeError('name')
if not name:
return SEP
elif name[-1] == SEP:
name = name[:-1]
return name[:name.rfind(SEP)+1] or SEP
def ns_join(ns, name):
"""
Join a namespace and name. If name is unjoinable (i.e. ~private or
/global) it will be returned without joining
@param ns: namespace ('/' and '~' are both legal). If ns is the empty string, name will be returned.
@type ns: str
@param name str: a legal name
@return str: name concatenated to ns, or name if it is
unjoinable.
@rtype: str
"""
if is_private(name) or is_global(name):
return name
if ns == PRIV_NAME:
return PRIV_NAME + name
if not ns:
return name
if ns[-1] == SEP:
return ns + name
return ns + SEP + name
def load_mappings(argv):
"""
Load name mappings encoded in command-line arguments. This will filter
out any parameter assignment mappings.
@param argv: command-line arguments
@type argv: [str]
@return: name->name remappings.
@rtype: dict {str: str}
"""
mappings = {}
for arg in argv:
if REMAP in arg:
try:
src, dst = [x.strip() for x in arg.split(REMAP)]
if src and dst:
if len(src) > 1 and src[0] == '_' and src[1] != '_':
#ignore parameter assignment mappings
pass
else:
mappings[src] = dst
except:
#TODO: remove
sys.stderr.write("ERROR: Invalid remapping argument '%s'\n"%arg)
return mappings
################################################################################
# NAME VALIDATORS
import re
#~,/, or ascii char followed by (alphanumeric, _, /)
NAME_LEGAL_CHARS_P = re.compile('^[\~\/A-Za-z][\w\/]*$')
def is_legal_name(name):
"""
Check if name is a legal ROS name for graph resources
(alphabetical character followed by alphanumeric, underscore, or
forward slashes). This constraint is currently not being enforced,
but may start getting enforced in later versions of ROS.
@param name: Name
@type name: str
"""
# should we enforce unicode checks?
if name is None:
return False
# empty string is a legal name as it resolves to namespace
if name == '':
return True
m = NAME_LEGAL_CHARS_P.match(name)
return m is not None and m.group(0) == name and not '//' in name
BASE_NAME_LEGAL_CHARS_P = re.compile('^[A-Za-z][\w]*$') #ascii char followed by (alphanumeric, _)
def is_legal_base_name(name):
"""
Validates that name is a legal base name for a graph resource. A base name has
no namespace context, e.g. "node_name".
"""
if name is None:
return False
m = BASE_NAME_LEGAL_CHARS_P.match(name)
return m is not None and m.group(0) == name
def canonicalize_name(name):
"""
Put name in canonical form. Extra slashes '//' are removed and
name is returned without any trailing slash, e.g. /foo/bar
@param name: ROS name
@type name: str
"""
if not name or name == SEP:
return name
elif name[0] == SEP:
return '/' + '/'.join([x for x in name.split(SEP) if x])
else:
return '/'.join([x for x in name.split(SEP) if x])
def resolve_name(name, namespace_, remappings=None):
"""
Resolve a ROS name to its global, canonical form. Private ~names
are resolved relative to the node name.
@param name: name to resolve.
@type name: str
@param namespace_: node name to resolve relative to.
@type namespace_: str
@param remappings: Map of resolved remappings. Use None to indicate no remapping.
@return: Resolved name. If name is empty/None, resolve_name
returns parent namespace_. If namespace_ is empty/None,
@rtype: str
"""
if not name: #empty string resolves to parent of the namespace_
return namespace(namespace_)
name = canonicalize_name(name)
if name[0] == SEP: #global name
resolved_name = name
elif is_private(name): #~name
# #3044: be careful not to accidentally make rest of name global
resolved_name = canonicalize_name(namespace_ + SEP + name[1:])
else: #relative
resolved_name = namespace(namespace_) + name
#Mappings override general namespace-based resolution
# - do this before canonicalization as remappings are meant to
# match the name as specified in the code
if remappings and resolved_name in remappings:
return remappings[resolved_name]
else:
return resolved_name
def script_resolve_name(script_name, name):
"""
Name resolver for scripts. Supports :envvar:`ROS_NAMESPACE`. Does not
support remapping arguments.
:param name: name to resolve, ``str``
:param script_name: name of script. script_name must not
contain a namespace., ``str``
:returns: resolved name, ``str``
"""
if not name: #empty string resolves to namespace
return get_ros_namespace()
#Check for global name: /foo/name resolves to /foo/name
if is_global(name):
return name
#Check for private name: ~name resolves to /caller_id/name
elif is_private(name):
return ns_join(make_caller_id(script_name), name[1:])
return get_ros_namespace() + name
def anonymous_name(id):
"""
Generate a ROS-legal 'anonymous' name
@param id: prefix for anonymous name
@type id: str
"""
import socket, random
name = "%s_%s_%s_%s"%(id, socket.gethostname(), os.getpid(), random.randint(0, sys.maxsize))
# RFC 952 allows hyphens, IP addrs can have '.'s, both
# of which are illegal for ROS names. For good
# measure, screen ipv6 ':'.
name = name.replace('.', '_')
name = name.replace('-', '_')
return name.replace(':', '_')

424
thirdparty/ros/ros_comm/tools/rosgraph/src/rosgraph/network.py vendored Normal file
View File

@@ -0,0 +1,424 @@
# 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.
#
# Revision $Id: network.py 15125 2011-10-06 02:51:15Z kwc $
"""
Network APIs for ROS-based systems, including IP address and ROS
TCP header libraries. Because ROS-based runtimes must respect the
ROS_IP and ROS_HOSTNAME environment variables, ROS-specific APIs
are necessary for correctly retrieving local IP address
information.
"""
import logging
import os
import socket
import struct
import sys
import platform
try:
from cStringIO import StringIO #Python 2.x
python3 = 0
except ImportError:
from io import BytesIO #Python 3.x
python3 = 1
try:
import urllib.parse as urlparse
except ImportError:
import urlparse
from .rosenv import ROS_IP, ROS_HOSTNAME, ROS_IPV6
SIOCGIFCONF = 0x8912
SIOCGIFADDR = 0x8915
if platform.system() == 'FreeBSD':
SIOCGIFADDR = 0xc0206921
if platform.architecture()[0] == '64bit':
SIOCGIFCONF = 0xc0106924
else:
SIOCGIFCONF = 0xc0086924
logger = logging.getLogger('rosgraph.network')
def parse_http_host_and_port(url):
"""
Convenience routine to handle parsing and validation of HTTP URL
port due to the fact that Python only provides easy accessors in
Python 2.5 and later. Validation checks that the protocol and host
are set.
:param url: URL to parse, ``str``
:returns: hostname and port number in URL or 80 (default), ``(str, int)``
:raises: :exc:`ValueError` If the url does not validate
"""
# can't use p.port because that's only available in Python 2.5
if not url:
raise ValueError('not a valid URL')
p = urlparse.urlparse(url)
if not p[0] or not p[1]: #protocol and host
raise ValueError('not a valid URL')
if ':' in p[1]:
hostname, port = p[1].split(':')
port = int(port)
else:
hostname, port = p[1], 80
return hostname, port
def _is_unix_like_platform():
"""
:returns: true if the platform conforms to UNIX/POSIX-style APIs
@rtype: bool
"""
return platform.system() in ['Linux', 'FreeBSD', 'Darwin']
def get_address_override():
"""
:returns: ROS_IP/ROS_HOSTNAME override or None, ``str``
:raises: :exc:`ValueError` If ROS_IP/ROS_HOSTNAME/__ip/__hostname are invalidly specified
"""
# #998: check for command-line remappings first
# TODO IPV6: check for compatibility
for arg in sys.argv:
if arg.startswith('__hostname:=') or arg.startswith('__ip:='):
try:
_, val = arg.split(':=')
return val
except: #split didn't unpack properly
raise ValueError("invalid ROS command-line remapping argument '%s'"%arg)
# check ROS_HOSTNAME and ROS_IP environment variables, which are
# aliases for each other
if ROS_HOSTNAME in os.environ:
hostname = os.environ[ROS_HOSTNAME]
if hostname == '':
msg = 'invalid ROS_HOSTNAME (an empty string)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
else:
parts = urlparse.urlparse(hostname)
if parts.scheme:
msg = 'invalid ROS_HOSTNAME (protocol ' + ('and port ' if parts.port else '') + 'should not be included)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
elif hostname.find(':') != -1:
# this can not be checked with urlparse()
# since it does not extract the port for a hostname like "foo:1234"
msg = 'invalid ROS_HOSTNAME (port should not be included)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
return hostname
elif ROS_IP in os.environ:
ip = os.environ[ROS_IP]
if ip == '':
msg = 'invalid ROS_IP (an empty string)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
elif ip.find('://') != -1:
msg = 'invalid ROS_IP (protocol should not be included)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
elif ip.find('.') != -1 and ip.rfind(':') > ip.rfind('.'):
msg = 'invalid ROS_IP (port should not be included)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
elif ip.find('.') == -1 and ip.find(':') == -1:
msg = 'invalid ROS_IP (must be a valid IPv4 or IPv6 address)'
sys.stderr.write(msg + '\n')
logger.warn(msg)
return ip
return None
def is_local_address(hostname):
"""
:param hostname: host name/address, ``str``
:returns True: if hostname maps to a local address, False otherwise. False conditions include invalid hostnames.
"""
try:
if use_ipv6():
reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, 0, 0, socket.SOL_TCP)]
else:
reverse_ips = [host[4][0] for host in socket.getaddrinfo(hostname, 0, socket.AF_INET, 0, socket.SOL_TCP)]
except socket.error:
return False
local_addresses = ['localhost'] + get_local_addresses()
# 127. check is due to #1260
if ([ip for ip in reverse_ips if (ip.startswith('127.') or ip == '::1')] != []) or (set(reverse_ips) & set(local_addresses) != set()):
return True
return False
def get_local_address():
"""
:returns: default local IP address (e.g. eth0). May be overriden by ROS_IP/ROS_HOSTNAME/__ip/__hostname, ``str``
"""
override = get_address_override()
if override:
return override
addrs = get_local_addresses()
if len(addrs) == 1:
return addrs[0]
for addr in addrs:
# pick first non 127/8 address
if not addr.startswith('127.') and not addr == '::1':
return addr
else: # loopback
if use_ipv6():
return '::1'
else:
return '127.0.0.1'
# cache for performance reasons
_local_addrs = None
def get_local_addresses():
"""
:returns: known local addresses. Not affected by ROS_IP/ROS_HOSTNAME, ``[str]``
"""
# cache address data as it can be slow to calculate
global _local_addrs
if _local_addrs is not None:
return _local_addrs
local_addrs = None
if _is_unix_like_platform():
# unix-only branch
v4addrs = []
v6addrs = []
import netifaces
for iface in netifaces.interfaces():
try:
ifaddrs = netifaces.ifaddresses(iface)
except ValueError:
# even if interfaces() returns an interface name
# ifaddresses() might raise a ValueError
# https://bugs.launchpad.net/ubuntu/+source/netifaces/+bug/753009
continue
if socket.AF_INET in ifaddrs:
v4addrs.extend([addr['addr'] for addr in ifaddrs[socket.AF_INET]])
if socket.AF_INET6 in ifaddrs:
v6addrs.extend([addr['addr'] for addr in ifaddrs[socket.AF_INET6]])
if use_ipv6():
local_addrs = v6addrs + v4addrs
else:
local_addrs = v4addrs
else:
# cross-platform branch, can only resolve one address
if use_ipv6():
local_addrs = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, 0, 0, socket.SOL_TCP)]
else:
local_addrs = [host[4][0] for host in socket.getaddrinfo(socket.gethostname(), 0, socket.AF_INET, 0, socket.SOL_TCP)]
_local_addrs = local_addrs
return local_addrs
def use_ipv6():
return ROS_IPV6 in os.environ and os.environ[ROS_IPV6] == 'on'
def get_bind_address(address=None):
"""
:param address: (optional) address to compare against, ``str``
:returns: address TCP/IP sockets should use for binding. This is
generally 0.0.0.0, but if \a address or ROS_IP/ROS_HOSTNAME is set
to localhost it will return 127.0.0.1, ``str``
"""
if address is None:
address = get_address_override()
if address and (address == 'localhost' or address.startswith('127.') or address == '::1' ):
#localhost or 127/8
if use_ipv6():
return '::1'
elif address.startswith('127.'):
return address
else:
return '127.0.0.1' #loopback
else:
if use_ipv6():
return '::'
else:
return '0.0.0.0'
# #528: semi-complicated logic for determining XML-RPC URI
def get_host_name():
"""
Determine host-name for use in host-name-based addressing (e.g. XML-RPC URIs):
- if ROS_IP/ROS_HOSTNAME is set, use that address
- if the hostname returns a non-localhost value, use that
- use whatever L{get_local_address()} returns
"""
hostname = get_address_override()
if not hostname:
try:
hostname = socket.gethostname()
except:
pass
if not hostname or hostname == 'localhost' or hostname.startswith('127.'):
hostname = get_local_address()
return hostname
def create_local_xmlrpc_uri(port):
"""
Determine the XMLRPC URI for local servers. This handles the search
logic of checking ROS environment variables, the known hostname,
and local interface IP addresses to determine the best possible
URI.
:param port: port that server is running on, ``int``
:returns: XMLRPC URI, ``str``
"""
#TODO: merge logic in rosgraph.xmlrpc with this routine
# in the future we may not want to be locked to http protocol nor root path
return 'http://%s:%s/'%(get_host_name(), port)
## handshake utils ###########################################
class ROSHandshakeException(Exception):
"""
Exception to represent errors decoding handshake
"""
pass
def decode_ros_handshake_header(header_str):
"""
Decode serialized ROS handshake header into a Python dictionary
header is a list of string key=value pairs, each prefixed by a
4-byte length field. It is preceeded by a 4-byte length field for
the entire header.
:param header_str: encoded header string. May contain extra data at the end, ``str``
:returns: key value pairs encoded in \a header_str, ``{str: str}``
"""
(size, ) = struct.unpack('<I', header_str[0:4])
size += 4 # add in 4 to include size of size field
header_len = len(header_str)
if size > header_len:
raise ROSHandshakeException("Incomplete header. Expected %s bytes but only have %s"%((size+4), header_len))
d = {}
start = 4
while start < size:
(field_size, ) = struct.unpack('<I', header_str[start:start+4])
if field_size == 0:
raise ROSHandshakeException("Invalid 0-length handshake header field")
start += field_size + 4
if start > size:
raise ROSHandshakeException("Invalid line length in handshake header: %s"%size)
line = header_str[start-field_size:start]
#python3 compatibility
if python3 == 1:
line = line.decode()
idx = line.find("=")
if idx < 0:
raise ROSHandshakeException("Invalid line in handshake header: [%s]"%line)
key = line[:idx]
value = line[idx+1:]
d[key.strip()] = value
return d
def read_ros_handshake_header(sock, b, buff_size):
"""
Read in tcpros header off the socket \a sock using buffer \a b.
:param sock: socket must be in blocking mode, ``socket``
:param b: buffer to use, ``StringIO`` for Python2, ``BytesIO`` for Python 3
:param buff_size: incoming buffer size to use, ``int``
:returns: key value pairs encoded in handshake, ``{str: str}``
:raises: :exc:`ROSHandshakeException` If header format does not match expected
"""
header_str = None
while not header_str:
d = sock.recv(buff_size)
if not d:
raise ROSHandshakeException("connection from sender terminated before handshake header received. %s bytes were received. Please check sender for additional details."%b.tell())
b.write(d)
btell = b.tell()
if btell > 4:
# most likely we will get the full header in the first recv, so
# not worth tiny optimizations possible here
bval = b.getvalue()
(size,) = struct.unpack('<I', bval[0:4])
if btell - 4 >= size:
header_str = bval
# memmove the remnants of the buffer back to the start
leftovers = bval[size+4:]
b.truncate(len(leftovers))
b.seek(0)
b.write(leftovers)
header_recvd = True
# process the header
return decode_ros_handshake_header(bval)
def encode_ros_handshake_header(header):
"""
Encode ROS handshake header as a byte string. Each header
field is a string key value pair. The encoded header is
prefixed by a length field, as is each field key/value pair.
key/value pairs a separated by a '=' equals sign.
FORMAT: (4-byte length + [4-byte field length + field=value ]*)
:param header: header field keys/values, ``dict``
:returns: header encoded as byte string, ``bytes``
"""
str_cls = str if python3 else unicode
# encode all unicode keys in the header. Ideally, the type of these would be specified by the api
encoded_header = {}
for k, v in header.items():
if isinstance(k, str_cls):
k = k.encode('utf-8')
if isinstance(v, str_cls):
v = v.encode('utf-8')
encoded_header[k] = v
fields = [k + b"=" + v for k, v in sorted(encoded_header.items())]
s = b''.join([struct.pack('<I', len(f)) + f for f in fields])
return struct.pack('<I', len(s)) + s
def write_ros_handshake_header(sock, header):
"""
Write ROS handshake header header to socket sock
:param sock: socket to write to (must be in blocking mode), ``socket.socket``
:param header: header field keys/values, ``{str : str}``
:returns: Number of bytes sent (for statistics), ``int``
"""
s = encode_ros_handshake_header(header)
sock.sendall(s)
return len(s) #STATS

Some files were not shown because too many files have changed in this diff Show More