diff --git a/clients/roscpp/CMakeLists.txt b/clients/roscpp/CMakeLists.txt index 4545073d29..f4d85b67ca 100644 --- a/clients/roscpp/CMakeLists.txt +++ b/clients/roscpp/CMakeLists.txt @@ -109,7 +109,6 @@ add_library(roscpp src/libros/spinner.cpp src/libros/internal_timer_manager.cpp src/libros/message_deserializer.cpp - src/libros/header.cpp src/libros/poll_set.cpp src/libros/service.cpp src/libros/this_node.cpp diff --git a/clients/roscpp/include/ros/forwards.h b/clients/roscpp/include/ros/forwards.h index eb3bf03a4c..71286e63dc 100644 --- a/clients/roscpp/include/ros/forwards.h +++ b/clients/roscpp/include/ros/forwards.h @@ -41,6 +41,7 @@ #include #include #include "exceptions.h" +#include "ros/datatypes.h" namespace ros { @@ -88,11 +89,6 @@ typedef boost::shared_ptr TransportPtr; class NodeHandle; typedef boost::shared_ptr NodeHandlePtr; -typedef std::vector > VP_string; -typedef std::vector V_string; -typedef std::set S_string; -typedef std::map M_string; -typedef std::pair StringPair; class SingleSubscriberPublisher; typedef boost::function SubscriberStatusCallback; diff --git a/clients/roscpp/include/ros/header.h b/clients/roscpp/include/ros/header.h deleted file mode 100644 index f9056ed1cd..0000000000 --- a/clients/roscpp/include/ros/header.h +++ /dev/null @@ -1,95 +0,0 @@ -/* - * 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 ROSCPP_HEADER_H -#define ROSCPP_HEADER_H - -#include - -#include -#include -#include "common.h" - -#include - -namespace ros -{ - -typedef std::map M_string; -typedef boost::shared_ptr M_stringPtr; - -class Transport; -typedef boost::shared_ptr TransportPtr; - -/** - * \brief Provides functionality to parse a connection header and retrieve values from it - */ -class ROSCPP_DECL Header -{ -public: - Header(); - ~Header(); - - /** - * \brief Get a value from a parsed header - * \param key Key value - * \param value OUT -- value corresponding to the key if there is one - * \return Returns true if the header had the specified key in it - */ - bool getValue(const std::string& key, std::string& value) const; - /** - * \brief Returns a shared pointer to the internal map used - */ - M_stringPtr getValues() { return read_map_; } - - /** - * \brief Parse a header out of a buffer of data - */ - bool parse(const boost::shared_array& buffer, uint32_t size, std::string& error_msg); - - /** - * \brief Parse a header out of a buffer of data - */ - bool parse(uint8_t* buffer, uint32_t size, std::string& error_msg); - - static void write(const M_string& key_vals, boost::shared_array& buffer, uint32_t& size); - -private: - - M_stringPtr read_map_; -}; - -} - -#endif // ROSCPP_HEADER_H diff --git a/clients/roscpp/include/ros/message_event.h b/clients/roscpp/include/ros/message_event.h deleted file mode 100644 index 5ee476b7c0..0000000000 --- a/clients/roscpp/include/ros/message_event.h +++ /dev/null @@ -1,272 +0,0 @@ - -/* - * Copyright (C) 2010, 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 ROSCPP_MESSAGE_EVENT_H -#define ROSCPP_MESSAGE_EVENT_H - -#include "ros/forwards.h" -#include "ros/time.h" -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace ros -{ - -template -struct DefaultMessageCreator -{ - boost::shared_ptr operator()() - { - return boost::make_shared(); - } -}; - -template -ROS_DEPRECATED inline boost::shared_ptr defaultMessageCreateFunction() -{ - return DefaultMessageCreator()(); -} - -/** - * \brief Event type for subscriptions, const ros::MessageEvent& can be used in your callback instead of const boost::shared_ptr& - * - * Useful if you need to retrieve meta-data about the message, such as the full connection header, or the publisher's node name - */ -template -class MessageEvent -{ -public: - typedef typename boost::add_const::type ConstMessage; - typedef typename boost::remove_const::type Message; - typedef boost::shared_ptr MessagePtr; - typedef boost::shared_ptr ConstMessagePtr; - typedef boost::function CreateFunction; - - MessageEvent() - : nonconst_need_copy_(true) - {} - - MessageEvent(const MessageEvent& rhs) - { - *this = rhs; - } - - MessageEvent(const MessageEvent& rhs) - { - *this = rhs; - } - - MessageEvent(const MessageEvent& rhs, bool nonconst_need_copy) - { - *this = rhs; - nonconst_need_copy_ = nonconst_need_copy; - } - - MessageEvent(const MessageEvent& rhs, bool nonconst_need_copy) - { - *this = rhs; - nonconst_need_copy_ = nonconst_need_copy; - } - - MessageEvent(const MessageEvent& rhs, const CreateFunction& create) - { - init(boost::const_pointer_cast(boost::static_pointer_cast(rhs.getMessage())), rhs.getConnectionHeaderPtr(), rhs.getReceiptTime(), rhs.nonConstWillCopy(), create); - } - - /** - * \todo Make this explicit in ROS 2.0. Keep as auto-converting for now to maintain backwards compatibility in some places (message_filters) - */ - MessageEvent(const ConstMessagePtr& message) - { - init(message, getConnectionHeader(message.get()), ros::Time::now(), true, ros::DefaultMessageCreator()); - } - - MessageEvent(const ConstMessagePtr& message, const boost::shared_ptr& connection_header, ros::Time receipt_time) - { - init(message, connection_header, receipt_time, true, ros::DefaultMessageCreator()); - } - - MessageEvent(const ConstMessagePtr& message, ros::Time receipt_time) - { - init(message, getConnectionHeader(message.get()), receipt_time, true, ros::DefaultMessageCreator()); - } - - MessageEvent(const ConstMessagePtr& message, const boost::shared_ptr& connection_header, ros::Time receipt_time, bool nonconst_need_copy, const CreateFunction& create) - { - init(message, connection_header, receipt_time, nonconst_need_copy, create); - } - - void init(const ConstMessagePtr& message, const boost::shared_ptr& connection_header, ros::Time receipt_time, bool nonconst_need_copy, const CreateFunction& create) - { - message_ = message; - connection_header_ = connection_header; - receipt_time_ = receipt_time; - nonconst_need_copy_ = nonconst_need_copy; - create_ = create; - } - - void operator=(const MessageEvent& rhs) - { - init(boost::static_pointer_cast(rhs.getMessage()), rhs.getConnectionHeaderPtr(), rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); - message_copy_.reset(); - } - - void operator=(const MessageEvent& rhs) - { - init(boost::const_pointer_cast(boost::static_pointer_cast(rhs.getMessage())), rhs.getConnectionHeaderPtr(), rhs.getReceiptTime(), rhs.nonConstWillCopy(), rhs.getMessageFactory()); - message_copy_.reset(); - } - - /** - * \brief Retrieve the message. If M is const, this returns a reference to it. If M is non const - * and this event requires it, returns a copy. Note that it caches this copy for later use, so it will - * only every make the copy once - */ - boost::shared_ptr getMessage() const - { - return copyMessageIfNecessary(); - } - - /** - * \brief Retrieve a const version of the message - */ - const boost::shared_ptr& getConstMessage() const { return message_; } - /** - * \brief Retrieve the connection header - */ - M_string& getConnectionHeader() const { return *connection_header_; } - const boost::shared_ptr& getConnectionHeaderPtr() const { return connection_header_; } - - /** - * \brief Returns the name of the node which published this message - */ - const std::string& getPublisherName() const { return connection_header_ ? (*connection_header_)["callerid"] : s_unknown_publisher_string_; } - - /** - * \brief Returns the time at which this message was received - */ - ros::Time getReceiptTime() const { return receipt_time_; } - - bool nonConstWillCopy() const { return nonconst_need_copy_; } - bool getMessageWillCopy() const { return !boost::is_const::value && nonconst_need_copy_; } - - bool operator<(const MessageEvent& rhs) - { - if (message_ != rhs.message_) - { - return message_ < rhs.message_; - } - - if (receipt_time_ != rhs.receipt_time_) - { - return receipt_time_ < rhs.receipt_time_; - } - - return nonconst_need_copy_ < rhs.nonconst_need_copy_; - } - - bool operator==(const MessageEvent& rhs) - { - return message_ = rhs.message_ && receipt_time_ == rhs.receipt_time_ && nonconst_need_copy_ == rhs.nonconst_need_copy_; - } - - bool operator!=(const MessageEvent& rhs) - { - return !(*this == rhs); - } - - const CreateFunction& getMessageFactory() const { return create_; } - -private: - template - typename boost::disable_if, boost::shared_ptr >::type copyMessageIfNecessary() const - { - if (boost::is_const::value || !nonconst_need_copy_) - { - return boost::const_pointer_cast(message_); - } - - if (message_copy_) - { - return message_copy_; - } - - ROS_ASSERT(create_); - message_copy_ = create_(); - *message_copy_ = *message_; - - return message_copy_; - } - - template - typename boost::enable_if, boost::shared_ptr >::type copyMessageIfNecessary() const - { - return boost::const_pointer_cast(message_); - } - - template - boost::shared_ptr - getConnectionHeader(T* t, typename boost::enable_if >::type*_ = 0) const - { - return t->__connection_header; - } - - template - boost::shared_ptr - getConnectionHeader(T* t, typename boost::disable_if >::type*_ = 0) const - { - return boost::shared_ptr(); - } - - - ConstMessagePtr message_; - // Kind of ugly to make this mutable, but it means we can pass a const MessageEvent to a callback and not worry about other things being modified - mutable MessagePtr message_copy_; - boost::shared_ptr connection_header_; - ros::Time receipt_time_; - bool nonconst_need_copy_; - CreateFunction create_; - - static const std::string s_unknown_publisher_string_; -}; - -template const std::string MessageEvent::s_unknown_publisher_string_("unknown_publisher"); - - -} - -#endif // ROSCPP_MESSAGE_EVENT_H diff --git a/clients/roscpp/include/ros/subscription_callback_helper.h b/clients/roscpp/include/ros/subscription_callback_helper.h index 7c8c5a3df6..c30b8903f6 100644 --- a/clients/roscpp/include/ros/subscription_callback_helper.h +++ b/clients/roscpp/include/ros/subscription_callback_helper.h @@ -49,46 +49,6 @@ namespace ros { -namespace serialization -{ -// Additional serialization traits - -template -struct PreDeserializeParams -{ - boost::shared_ptr message; - boost::shared_ptr > connection_header; -}; - -/** - * \brief called by the SubscriptionCallbackHelper after a message is - * instantiated but before that message is deserialized - */ -template -struct PreDeserialize -{ - static void notify(const PreDeserializeParams&) { } -}; - -} - -template -void -assignSubscriptionConnectionHeader(T* t, const boost::shared_ptr& connection_header, - typename boost::enable_if >::type*_=0) -{ - (void)_; // warning stopper - t->__connection_header = connection_header; -} - -template -void -assignSubscriptionConnectionHeader(T* t, const boost::shared_ptr& connection_header, - typename boost::disable_if >::type*_=0) -{ - (void)_; // warning stopper -} - struct SubscriptionCallbackHelperDeserializeParams { uint8_t* buffer; diff --git a/clients/roscpp/include/ros/subscription_queue.h b/clients/roscpp/include/ros/subscription_queue.h index 7f68b04bf4..1df0d5fc3e 100644 --- a/clients/roscpp/include/ros/subscription_queue.h +++ b/clients/roscpp/include/ros/subscription_queue.h @@ -30,7 +30,7 @@ #include "forwards.h" #include "common.h" -#include "message_event.h" +#include "ros/message_event.h" #include "callback_queue_interface.h" #include diff --git a/clients/roscpp/src/libros/header.cpp b/clients/roscpp/src/libros/header.cpp deleted file mode 100644 index aefb376290..0000000000 --- a/clients/roscpp/src/libros/header.cpp +++ /dev/null @@ -1,170 +0,0 @@ -/* - * 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/header.h" -#include "ros/transport/transport.h" -#include -#include - -#include - -#include - -#define SROS_SERIALIZE_PRIMITIVE(ptr, data) { memcpy(ptr, &data, sizeof(data)); ptr += sizeof(data); } -#define SROS_SERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(ptr, data, data_size); ptr += data_size; } } -#define SROS_DESERIALIZE_PRIMITIVE(ptr, data) { memcpy(&data, ptr, sizeof(data)); ptr += sizeof(data); } -#define SROS_DESERIALIZE_BUFFER(ptr, data, data_size) { if (data_size > 0) { memcpy(data, ptr, data_size); ptr += data_size; } } - - -using namespace std; - -namespace ros -{ - -Header::Header() -: read_map_(new M_string()) -{ - -} - -Header::~Header() -{ - -} - -bool Header::parse(const boost::shared_array& buffer, uint32_t size, std::string& error_msg) -{ - return parse(buffer.get(), size, error_msg); -} - -bool Header::parse(uint8_t* buffer, uint32_t size, std::string& error_msg) -{ - uint8_t* buf = buffer; - while (buf < buffer + size) - { - uint32_t len; - SROS_DESERIALIZE_PRIMITIVE(buf, len); - - if (len > 1000000) - { - error_msg = "Received an invalid TCPROS header. Each element must be prepended by a 4-byte length."; - ROS_ERROR("%s", error_msg.c_str()); - - return false; - } - - std::string line((char*)buf, len); - - buf += len; - - //printf(":%s:\n", line.c_str()); - size_t eqpos = line.find_first_of("=", 0); - if (eqpos == string::npos) - { - error_msg = "Received an invalid TCPROS header. Each line must have an equals sign."; - ROS_ERROR("%s", error_msg.c_str()); - - return false; - } - string key = line.substr(0, eqpos); - string value = line.substr(eqpos+1); - - (*read_map_)[key] = value; - } - - return true; -} - -bool Header::getValue(const std::string& key, std::string& value) const -{ - M_string::const_iterator it = read_map_->find(key); - if (it == read_map_->end()) - { - return false; - } - - value = it->second; - - return true; -} - -void Header::write(const M_string& key_vals, boost::shared_array& buffer, uint32_t& size) -{ - // Calculate the necessary size - size = 0; - { - M_string::const_iterator it = key_vals.begin(); - M_string::const_iterator end = key_vals.end(); - for (; it != end; ++it) - { - const std::string& key = it->first; - const std::string& value = it->second; - - size += key.length(); - size += value.length(); - size += 1; // = sign - size += 4; // 4-byte length - } - } - - if (size == 0) - { - return; - } - - buffer.reset(new uint8_t[size]); - char* ptr = (char*)buffer.get(); - - // Write the data - { - M_string::const_iterator it = key_vals.begin(); - M_string::const_iterator end = key_vals.end(); - for (; it != end; ++it) - { - const std::string& key = it->first; - const std::string& value = it->second; - - uint32_t len = key.length() + value.length() + 1; - SROS_SERIALIZE_PRIMITIVE(ptr, len); - SROS_SERIALIZE_BUFFER(ptr, key.data(), key.length()); - static const char equals = '='; - SROS_SERIALIZE_PRIMITIVE(ptr, equals); - SROS_SERIALIZE_BUFFER(ptr, value.data(), value.length()); - } - } - - ROS_ASSERT(ptr == (char*)buffer.get() + size); -} - -} diff --git a/clients/roscpp/src/libros/io.cpp b/clients/roscpp/src/libros/io.cpp index a659d40749..6f0c242865 100644 --- a/clients/roscpp/src/libros/io.cpp +++ b/clients/roscpp/src/libros/io.cpp @@ -35,7 +35,7 @@ ** Includes *****************************************************************************/ -#include "../../include/ros/io.h" +#include #include // don't need if we dont call the pipe functions. #include // for EFAULT and co. #include diff --git a/test/test_rosbag/test/test_bag.cpp.in b/test/test_rosbag/test/test_bag.cpp.in index 56d8cada60..c26d39d21b 100644 --- a/test/test_rosbag/test/test_bag.cpp.in +++ b/test/test_rosbag/test/test_bag.cpp.in @@ -25,6 +25,7 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. +#include "ros/ros.h" #include "rosbag/bag.h" #include "rosbag/chunked_file.h" #include "rosbag/view.h" diff --git a/test/test_rosbag_storage/CMakeLists.txt b/test/test_rosbag_storage/CMakeLists.txt new file mode 100644 index 0000000000..b9e834e02c --- /dev/null +++ b/test/test_rosbag_storage/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(test_rosbag_storage) + +find_package(catkin REQUIRED COMPONENTS rosbag_storage std_msgs) + +find_package(Boost REQUIRED) + +catkin_package() + +include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + +add_executable(create_bag src/create_bag.cpp) +target_link_libraries(create_bag ${catkin_LIBRARIES}) + +add_executable(iterate_bag src/iterate_bag.cpp) +target_link_libraries(iterate_bag ${catkin_LIBRARIES}) diff --git a/test/test_rosbag_storage/package.xml b/test/test_rosbag_storage/package.xml new file mode 100644 index 0000000000..2e8d25e893 --- /dev/null +++ b/test/test_rosbag_storage/package.xml @@ -0,0 +1,17 @@ + + test_rosbag_storage + 1.9.50 + + A package containing the unit tests for rosbag_storage. + + Dirk Thomas + BSD + + catkin + + rosbag_storage + std_msgs + + rosbag_storage + std_msgs + diff --git a/test/test_rosbag_storage/src/create_bag.cpp b/test/test_rosbag_storage/src/create_bag.cpp new file mode 100644 index 0000000000..62a57f8c53 --- /dev/null +++ b/test/test_rosbag_storage/src/create_bag.cpp @@ -0,0 +1,28 @@ +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include "std_msgs/String.h" +#include "std_msgs/Int32.h" +#include "ros/time.h" + +int main(void) +{ + + ros::Time::init(); + + 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(); + + + return 0; +} diff --git a/test/test_rosbag_storage/src/iterate_bag.cpp b/test/test_rosbag_storage/src/iterate_bag.cpp new file mode 100644 index 0000000000..3e6e168c8e --- /dev/null +++ b/test/test_rosbag_storage/src/iterate_bag.cpp @@ -0,0 +1,48 @@ +#include "rosbag/bag.h" +#include "rosbag/view.h" +#include +#include "std_msgs/String.h" +#include "std_msgs/Int32.h" +#include "boost/foreach.hpp" + +#include "ros/time.h" + +int main(void) +{ + + ros::Time::init(); + + rosbag::Bag bag; + bag.open("test.bag", rosbag::bagmode::Read); + + std::vector topics; + topics.push_back(std::string("chatter")); + topics.push_back(std::string("numbers")); + + rosbag::View view(bag, rosbag::TopicQuery(topics)); + + BOOST_FOREACH(rosbag::MessageInstance const m, view) + { + std_msgs::String::ConstPtr s = m.instantiate(); + if (s != NULL) + if(s->data == std::string("foo")) { + printf("Successfully checked string foo\n"); + } + else { + printf("Failed checked string foo\n"); + } + + std_msgs::Int32::ConstPtr i = m.instantiate(); + if (i != NULL) + if (i->data == 42) { + printf("Successfully checked value 42\n"); + } + else { + printf("Failed checked value 42.\n"); + } +; + } + + bag.close(); + return 0; +} diff --git a/tools/rosbag/CMakeLists.txt b/tools/rosbag/CMakeLists.txt index 2e89f76440..05a854509f 100644 --- a/tools/rosbag/CMakeLists.txt +++ b/tools/rosbag/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 2.8.3) project(rosbag) -find_package(catkin REQUIRED COMPONENTS rosconsole roscpp topic_tools xmlrpcpp) +find_package(catkin REQUIRED COMPONENTS rosbag_storage rosconsole roscpp topic_tools xmlrpcpp) find_package(Boost REQUIRED COMPONENTS date_time regex program_options filesystem) find_package(BZip2 REQUIRED) @@ -9,28 +9,19 @@ catkin_python_setup() # Support large bags (>2GB) on 32-bit systems add_definitions(-D_FILE_OFFSET_BITS=64) -include_directories(include ${Boost_INCLUDE_DIR} ${BZIP2_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) -add_definitions(${BZIP2_DEFINITIONS}) +include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}) catkin_package( LIBRARIES rosbag INCLUDE_DIRS include - CATKIN_DEPENDS rosconsole roscpp topic_tools xmlrpcpp) + CATKIN_DEPENDS rosbag_storage rosconsole roscpp topic_tools xmlrpcpp) add_library(rosbag - src/bag.cpp - src/buffer.cpp - src/bz2_stream.cpp - src/chunked_file.cpp - src/message_instance.cpp src/player.cpp - src/query.cpp src/recorder.cpp - src/stream.cpp - src/time_translator.cpp - src/uncompressed_stream.cpp - src/view.cpp) -target_link_libraries(rosbag ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${BZIP2_LIBRARIES}) + src/time_translator.cpp) + +target_link_libraries(rosbag ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_executable(record src/record.cpp) target_link_libraries(record rosbag) diff --git a/tools/rosbag/include/rosbag/player.h b/tools/rosbag/include/rosbag/player.h index 791cc1492c..aebbf99258 100644 --- a/tools/rosbag/include/rosbag/player.h +++ b/tools/rosbag/include/rosbag/player.h @@ -51,10 +51,19 @@ #include "rosbag/bag.h" #include "rosbag/time_translator.h" -#include "macros.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 + */ +ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& msg, uint32_t queue_size); + + + struct ROSBAG_DECL PlayerOptions { PlayerOptions(); diff --git a/tools/rosbag/include/rosbag/time_translator.h b/tools/rosbag/include/rosbag/time_translator.h index 6951a18446..df18b68bfe 100644 --- a/tools/rosbag/include/rosbag/time_translator.h +++ b/tools/rosbag/include/rosbag/time_translator.h @@ -36,7 +36,7 @@ #define ROSBAG_TIME_TRANSLATOR_H #include "ros/time.h" -#include "macros.h" +#include "rosbag/macros.h" namespace rosbag { diff --git a/tools/rosbag/package.xml b/tools/rosbag/package.xml index 6765236266..a8ff0048b8 100644 --- a/tools/rosbag/package.xml +++ b/tools/rosbag/package.xml @@ -17,9 +17,9 @@ catkin boost - bzip2 cpp_common python-imaging + rosbag_storage rosconsole roscpp roscpp_serialization @@ -27,10 +27,10 @@ xmlrpcpp boost - bzip2 genmsg genpy python-rospkg + rosbag_storage rosconsole roscpp roslib diff --git a/tools/rosbag/src/player.cpp b/tools/rosbag/src/player.cpp index 4745a1de1d..fa6245da44 100644 --- a/tools/rosbag/src/player.cpp +++ b/tools/rosbag/src/player.cpp @@ -56,6 +56,18 @@ using ros::Exception; namespace rosbag { +ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size) { + ros::AdvertiseOptions opts(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) { + return ros::AdvertiseOptions(m.getTopic(), queue_size, m.getMD5Sum(), m.getDataType(), m.getMessageDefinition()); +} + // PlayerOptions PlayerOptions::PlayerOptions() : diff --git a/tools/rosbag_storage/CMakeLists.txt b/tools/rosbag_storage/CMakeLists.txt new file mode 100644 index 0000000000..6394e73bf4 --- /dev/null +++ b/tools/rosbag_storage/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 2.8.3) + +project(rosbag_storage) + +find_package(console_bridge REQUIRED) +find_package(catkin REQUIRED COMPONENTS cpp_common roscpp_serialization roscpp_traits rostime) +find_package(Boost REQUIRED COMPONENTS date_time filesystem program_options regex) +find_package(BZip2 REQUIRED) + +catkin_package( + INCLUDE_DIRS include + LIBRARIES rosbag_storage + DEPENDS console_bridge Boost +) + +include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ${BZIP2_INCLUDE_DIR} ${console_bridge__INCLUDE_DIRS}) +add_definitions(${BZIP2_DEFINITIONS}) + +add_library(rosbag_storage + src/bag.cpp + src/bag_player.cpp + src/buffer.cpp + src/bz2_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" +) diff --git a/tools/rosbag/include/rosbag/bag.h b/tools/rosbag_storage/include/rosbag/bag.h similarity index 98% rename from tools/rosbag/include/rosbag/bag.h rename to tools/rosbag_storage/include/rosbag/bag.h index 200a79865d..e3e388b752 100644 --- a/tools/rosbag/include/rosbag/bag.h +++ b/tools/rosbag_storage/include/rosbag/bag.h @@ -36,6 +36,7 @@ #define ROSBAG_BAG_H #include "rosbag/macros.h" + #include "rosbag/buffer.h" #include "rosbag/chunked_file.h" #include "rosbag/constants.h" @@ -45,8 +46,10 @@ #include "ros/header.h" #include "ros/time.h" #include "ros/message_traits.h" -#include "ros/subscription_callback_helper.h" -#include "ros/ros.h" +#include "ros/message_event.h" +#include "ros/serialization.h" + +//#include "ros/subscription_callback_helper.h" #include #include @@ -57,6 +60,8 @@ #include #include +#include "console_bridge/console.h" + namespace rosbag { namespace bagmode @@ -564,7 +569,7 @@ void Bag::doWrite(std::string const& topic, ros::Time const& time, T const& msg, // Check if we want to stop this chunk uint32_t chunk_size = getChunkOffset(); - ROS_DEBUG(" curr_chunk_size=%d (threshold=%d)", chunk_size, chunk_threshold_); + logDebug(" curr_chunk_size=%d (threshold=%d)", chunk_size, chunk_threshold_); if (chunk_size > chunk_threshold_) { // Empty the outgoing chunk stopWritingChunk(); @@ -599,7 +604,7 @@ void Bag::writeMessageDataRecord(uint32_t conn_id, ros::Time const& time, T cons seek(0, std::ios::end); file_size_ = file_.getOffset(); - ROS_DEBUG("Writing MSG_DATA [%llu:%d]: conn=%d sec=%d nsec=%d data_len=%d", + 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); diff --git a/tools/rosbag_storage/include/rosbag/bag_player.h b/tools/rosbag_storage/include/rosbag/bag_player.h new file mode 100644 index 0000000000..e2185808dd --- /dev/null +++ b/tools/rosbag_storage/include/rosbag/bag_player.h @@ -0,0 +1,134 @@ +/********************************************************************* +* 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 + +#include "rosbag/bag.h" +#include "rosbag/view.h" + +namespace rosbag +{ + + +// A helper struct +struct BagCallback +{ + virtual void call(MessageInstance m) = 0; +}; + +// A helper class for the callbacks +template +class BagCallbackT : public BagCallback +{ +public: + typedef boost::function&)> Callback; + + BagCallbackT(Callback cb) : + cb_(cb) + {} + + void call(MessageInstance m) { + cb_(m.instantiate()); + } + +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) throw(BagException); + + /* Register a callback for a specific topic and type */ + template + void register_callback(const std::string &topic, + typename BagCallbackT::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 cbs_; + ros::Time bag_start_; + ros::Time bag_end_; + ros::Time last_message_time_; + double playback_speed_; + ros::Time play_start_; +}; + +template +void BagPlayer::register_callback(const std::string &topic, + typename BagCallbackT::Callback cb) { + cbs_[topic] = new BagCallbackT(cb); +} + +} + +#endif diff --git a/tools/rosbag/include/rosbag/buffer.h b/tools/rosbag_storage/include/rosbag/buffer.h similarity index 100% rename from tools/rosbag/include/rosbag/buffer.h rename to tools/rosbag_storage/include/rosbag/buffer.h diff --git a/tools/rosbag/include/rosbag/chunked_file.h b/tools/rosbag_storage/include/rosbag/chunked_file.h similarity index 100% rename from tools/rosbag/include/rosbag/chunked_file.h rename to tools/rosbag_storage/include/rosbag/chunked_file.h diff --git a/tools/rosbag/include/rosbag/constants.h b/tools/rosbag_storage/include/rosbag/constants.h similarity index 100% rename from tools/rosbag/include/rosbag/constants.h rename to tools/rosbag_storage/include/rosbag/constants.h diff --git a/tools/rosbag/include/rosbag/exceptions.h b/tools/rosbag_storage/include/rosbag/exceptions.h similarity index 100% rename from tools/rosbag/include/rosbag/exceptions.h rename to tools/rosbag_storage/include/rosbag/exceptions.h diff --git a/tools/rosbag/include/rosbag/macros.h b/tools/rosbag_storage/include/rosbag/macros.h similarity index 100% rename from tools/rosbag/include/rosbag/macros.h rename to tools/rosbag_storage/include/rosbag/macros.h diff --git a/tools/rosbag/include/rosbag/message_instance.h b/tools/rosbag_storage/include/rosbag/message_instance.h similarity index 93% rename from tools/rosbag/include/rosbag/message_instance.h rename to tools/rosbag_storage/include/rosbag/message_instance.h index e8d65e3e30..7f18ec6fdc 100644 --- a/tools/rosbag/include/rosbag/message_instance.h +++ b/tools/rosbag_storage/include/rosbag/message_instance.h @@ -36,11 +36,12 @@ #define ROSBAG_MESSAGE_INSTANCE_H #include -#include +#include +//#include #include #include "rosbag/structures.h" -#include "macros.h" +#include "rosbag/macros.h" namespace rosbag { @@ -101,13 +102,6 @@ class ROSBAG_DECL MessageInstance }; -//! 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 - */ -ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& msg, uint32_t queue_size); - } // namespace rosbag namespace ros { diff --git a/tools/rosbag/include/rosbag/query.h b/tools/rosbag_storage/include/rosbag/query.h similarity index 100% rename from tools/rosbag/include/rosbag/query.h rename to tools/rosbag_storage/include/rosbag/query.h diff --git a/tools/rosbag/include/rosbag/stream.h b/tools/rosbag_storage/include/rosbag/stream.h similarity index 100% rename from tools/rosbag/include/rosbag/stream.h rename to tools/rosbag_storage/include/rosbag/stream.h diff --git a/tools/rosbag/include/rosbag/structures.h b/tools/rosbag_storage/include/rosbag/structures.h similarity index 96% rename from tools/rosbag/include/rosbag/structures.h rename to tools/rosbag_storage/include/rosbag/structures.h index ed3ac79548..ad75b1ee95 100644 --- a/tools/rosbag/include/rosbag/structures.h +++ b/tools/rosbag_storage/include/rosbag/structures.h @@ -39,7 +39,7 @@ #include #include "ros/time.h" -#include "ros/ros.h" +#include "ros/datatypes.h" #include "macros.h" namespace rosbag { @@ -57,8 +57,6 @@ struct ROSBAG_DECL ConnectionInfo boost::shared_ptr header; }; -ROSBAG_DECL ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size); - struct ChunkInfo { ros::Time start_time; //! earliest timestamp of a message in the chunk diff --git a/tools/rosbag/include/rosbag/view.h b/tools/rosbag_storage/include/rosbag/view.h similarity index 100% rename from tools/rosbag/include/rosbag/view.h rename to tools/rosbag_storage/include/rosbag/view.h diff --git a/tools/rosbag_storage/package.xml b/tools/rosbag_storage/package.xml new file mode 100644 index 0000000000..2f873ce4cd --- /dev/null +++ b/tools/rosbag_storage/package.xml @@ -0,0 +1,28 @@ + + rosbag_storage + 1.9.50 + + This is a set of tools for recording from and playing back ROS + message without relying on the ROS client library. + + Dirk Thomas + BSD + + catkin + + boost + bzip2 + console_bridge + cpp_common + roscpp_serialization + roscpp_traits + rostime + + boost + bzip2 + console_bridge + cpp_common + roscpp_serialization + roscpp_traits + rostime + diff --git a/tools/rosbag/src/bag.cpp b/tools/rosbag_storage/src/bag.cpp similarity index 91% rename from tools/rosbag/src/bag.cpp rename to tools/rosbag_storage/src/bag.cpp index cb5679d8d4..efcfd24e5b 100644 --- a/tools/rosbag/src/bag.cpp +++ b/tools/rosbag_storage/src/bag.cpp @@ -36,11 +36,13 @@ #include #endif #include - +#include #include #include +#include "console_bridge/console.h" + #define foreach BOOST_FOREACH using std::map; @@ -200,7 +202,7 @@ void Bag::setCompression(CompressionType compression) { void Bag::writeVersion() { string version = string("#ROSBAG V") + VERSION + string("\n"); - ROS_DEBUG("Writing VERSION [%llu]: %s", (unsigned long long) file_.getOffset(), version.c_str()); + logDebug("Writing VERSION [%llu]: %s", (unsigned long long) file_.getOffset(), version.c_str()); version_ = 200; @@ -223,7 +225,7 @@ void Bag::readVersion() { version_ = version_major * 100 + version_minor; - ROS_DEBUG("Read VERSION: version=%d", version_); + logDebug("Read VERSION: version=%d", version_); } uint32_t Bag::getMajorVersion() const { return version_ / 100; } @@ -312,7 +314,7 @@ void Bag::startReadingVersion102() { multiset const& index = i->second; IndexEntry const& first_entry = *index.begin(); - ROS_DEBUG("Reading message definition for connection %d at %llu", i->first, (unsigned long long) first_entry.chunk_pos); + logDebug("Reading message definition for connection %d at %llu", i->first, (unsigned long long) first_entry.chunk_pos); seek(first_entry.chunk_pos); @@ -326,7 +328,7 @@ void Bag::writeFileHeaderRecord() { connection_count_ = connections_.size(); chunk_count_ = chunks_.size(); - ROS_DEBUG("Writing FILE_HEADER [%llu]: index_pos=%llu connection_count=%d chunk_count=%d", + logDebug("Writing FILE_HEADER [%llu]: index_pos=%llu connection_count=%d chunk_count=%d", (unsigned long long) file_.getOffset(), (unsigned long long) index_data_pos_, connection_count_, chunk_count_); // Write file header record @@ -377,7 +379,7 @@ void Bag::readFileHeaderRecord() { readField(fields, CHUNK_COUNT_FIELD_NAME, true, &chunk_count_); } - ROS_DEBUG("Read FILE_HEADER: index_pos=%llu connection_count=%d chunk_count=%d", + logDebug("Read FILE_HEADER: index_pos=%llu connection_count=%d chunk_count=%d", (unsigned long long) index_data_pos_, connection_count_, chunk_count_); // Skip the data section (just padding) @@ -446,7 +448,7 @@ void Bag::writeChunkHeader(CompressionType compression, uint32_t compressed_size chunk_header.compressed_size = compressed_size; chunk_header.uncompressed_size = uncompressed_size; - ROS_DEBUG("Writing CHUNK [%llu]: compression=%s compressed=%d uncompressed=%d", + logDebug("Writing CHUNK [%llu]: compression=%s compressed=%d uncompressed=%d", (unsigned long long) file_.getOffset(), chunk_header.compression.c_str(), chunk_header.compressed_size, chunk_header.uncompressed_size); M_string header; @@ -471,7 +473,7 @@ void Bag::readChunkHeader(ChunkHeader& chunk_header) const { readField(fields, COMPRESSION_FIELD_NAME, true, chunk_header.compression); readField(fields, SIZE_FIELD_NAME, true, &chunk_header.uncompressed_size); - ROS_DEBUG("Read CHUNK: compression=%s size=%d uncompressed=%d (%f)", chunk_header.compression.c_str(), chunk_header.compressed_size, chunk_header.uncompressed_size, 100 * ((double) chunk_header.compressed_size) / chunk_header.uncompressed_size); + logDebug("Read CHUNK: compression=%s size=%d uncompressed=%d (%f)", chunk_header.compression.c_str(), chunk_header.compressed_size, chunk_header.uncompressed_size, 100 * ((double) chunk_header.compressed_size) / chunk_header.uncompressed_size); } // Index records @@ -492,7 +494,7 @@ void Bag::writeIndexRecords() { writeDataLength(index_size * 12); - ROS_DEBUG("Writing INDEX_DATA: connection=%d ver=%d count=%d", connection_id, INDEX_VERSION, index_size); + logDebug("Writing INDEX_DATA: connection=%d ver=%d count=%d", connection_id, INDEX_VERSION, index_size); // Write the index record data (pairs of timestamp and position in file) foreach(IndexEntry const& e, index) { @@ -500,7 +502,7 @@ void Bag::writeIndexRecords() { write((char*) &e.time.nsec, 4); write((char*) &e.offset, 4); - ROS_DEBUG(" - %d.%d: %d", e.time.sec, e.time.nsec, e.offset); + logDebug(" - %d.%d: %d", e.time.sec, e.time.nsec, e.offset); } } } @@ -522,7 +524,7 @@ void Bag::readTopicIndexRecord102() { readField(fields, TOPIC_FIELD_NAME, true, topic); readField(fields, COUNT_FIELD_NAME, true, &count); - ROS_DEBUG("Read INDEX_DATA: ver=%d topic=%s count=%d", index_version, topic.c_str(), count); + logDebug("Read INDEX_DATA: ver=%d topic=%s count=%d", index_version, topic.c_str(), count); if (index_version != 0) throw BagFormatException((format("Unsupported INDEX_DATA version: %1%") % index_version).str()); @@ -532,7 +534,7 @@ void Bag::readTopicIndexRecord102() { if (topic_conn_id_iter == topic_connection_ids_.end()) { connection_id = connections_.size(); - ROS_DEBUG("Creating connection: id=%d topic=%s", connection_id, topic.c_str()); + logDebug("Creating connection: id=%d topic=%s", connection_id, topic.c_str()); ConnectionInfo* connection_info = new ConnectionInfo(); connection_info->id = connection_id; connection_info->topic = topic; @@ -555,11 +557,11 @@ void Bag::readTopicIndexRecord102() { index_entry.time = Time(sec, nsec); index_entry.offset = 0; - ROS_DEBUG(" - %d.%d: %llu", sec, nsec, (unsigned long long) index_entry.chunk_pos); + logDebug(" - %d.%d: %llu", sec, nsec, (unsigned long long) index_entry.chunk_pos); if (index_entry.time < ros::TIME_MIN || index_entry.time > ros::TIME_MAX) { - ROS_ERROR("Index entry for topic %s contains invalid time.", topic.c_str()); + logError("Index entry for topic %s contains invalid time.", topic.c_str()); } else { connection_index.insert(connection_index.end(), index_entry); @@ -584,7 +586,7 @@ void Bag::readConnectionIndexRecord200() { readField(fields, CONNECTION_FIELD_NAME, true, &connection_id); readField(fields, COUNT_FIELD_NAME, true, &count); - ROS_DEBUG("Read INDEX_DATA: ver=%d connection=%d count=%d", index_version, connection_id, count); + logDebug("Read INDEX_DATA: ver=%d connection=%d count=%d", index_version, connection_id, count); if (index_version != 1) throw BagFormatException((format("Unsupported INDEX_DATA version: %1%") % index_version).str()); @@ -603,11 +605,11 @@ void Bag::readConnectionIndexRecord200() { read((char*) &index_entry.offset, 4); index_entry.time = Time(sec, nsec); - ROS_DEBUG(" - %d.%d: %llu+%d", sec, nsec, (unsigned long long) index_entry.chunk_pos, index_entry.offset); + logDebug(" - %d.%d: %llu+%d", sec, nsec, (unsigned long long) index_entry.chunk_pos, index_entry.offset); if (index_entry.time < ros::TIME_MIN || index_entry.time > ros::TIME_MAX) { - ROS_ERROR("Index entry for topic %s contains invalid time. This message will not be loaded.", connections_[connection_id]->topic.c_str()); + logError("Index entry for topic %s contains invalid time. This message will not be loaded.", connections_[connection_id]->topic.c_str()); } else { connection_index.insert(connection_index.end(), index_entry); @@ -625,7 +627,7 @@ void Bag::writeConnectionRecords() { } void Bag::writeConnectionRecord(ConnectionInfo const* connection_info) { - ROS_DEBUG("Writing CONNECTION [%llu:%d]: topic=%s id=%d", + logDebug("Writing CONNECTION [%llu:%d]: topic=%s id=%d", (unsigned long long) file_.getOffset(), getChunkOffset(), connection_info->topic.c_str(), connection_info->id); M_string header; @@ -679,7 +681,7 @@ void Bag::readConnectionRecord() { connection_info->md5sum = (*connection_info->header)["md5sum"]; connections_[id] = connection_info; - ROS_DEBUG("Read CONNECTION: topic=%s id=%d", topic.c_str(), id); + logDebug("Read CONNECTION: topic=%s id=%d", topic.c_str(), id); } } @@ -705,7 +707,7 @@ void Bag::readMessageDefinitionRecord102() { if (topic_conn_id_iter == topic_connection_ids_.end()) { uint32_t id = connections_.size(); - ROS_DEBUG("Creating connection: topic=%s md5sum=%s datatype=%s", topic.c_str(), md5sum.c_str(), datatype.c_str()); + logDebug("Creating connection: topic=%s md5sum=%s datatype=%s", topic.c_str(), md5sum.c_str(), datatype.c_str()); connection_info = new ConnectionInfo(); connection_info->id = id; connection_info->topic = topic; @@ -724,7 +726,7 @@ void Bag::readMessageDefinitionRecord102() { (*connection_info->header)["md5sum"] = connection_info->md5sum; (*connection_info->header)["message_definition"] = connection_info->msg_def; - ROS_DEBUG("Read MSG_DEF: topic=%s md5sum=%s datatype=%s", topic.c_str(), md5sum.c_str(), datatype.c_str()); + logDebug("Read MSG_DEF: topic=%s md5sum=%s datatype=%s", topic.c_str(), md5sum.c_str(), datatype.c_str()); } void Bag::decompressChunk(uint64_t chunk_pos) const { @@ -757,7 +759,7 @@ void Bag::decompressChunk(uint64_t chunk_pos) const { } void Bag::readMessageDataRecord102(uint64_t offset, ros::Header& header) const { - ROS_DEBUG("readMessageDataRecord: offset=%llu", (unsigned long long) offset); + logDebug("readMessageDataRecord: offset=%llu", (unsigned long long) offset); seek(offset); @@ -783,7 +785,7 @@ void Bag::decompressRawChunk(ChunkHeader const& chunk_header) const { assert(chunk_header.compression == COMPRESSION_NONE); assert(chunk_header.compressed_size == chunk_header.uncompressed_size); - ROS_DEBUG("compressed_size: %d uncompressed_size: %d", chunk_header.compressed_size, chunk_header.uncompressed_size); + logDebug("compressed_size: %d uncompressed_size: %d", chunk_header.compressed_size, chunk_header.uncompressed_size); decompress_buffer_.setSize(chunk_header.compressed_size); file_.read((char*) decompress_buffer_.getData(), chunk_header.compressed_size); @@ -796,7 +798,7 @@ void Bag::decompressBz2Chunk(ChunkHeader const& chunk_header) const { CompressionType compression = compression::BZ2; - ROS_DEBUG("compressed_size: %d uncompressed_size: %d", chunk_header.compressed_size, chunk_header.uncompressed_size); + logDebug("compressed_size: %d uncompressed_size: %d", chunk_header.compressed_size, chunk_header.uncompressed_size); chunk_buffer_.setSize(chunk_header.compressed_size); file_.read((char*) chunk_buffer_.getData(), chunk_header.compressed_size); @@ -856,7 +858,7 @@ void Bag::writeChunkInfoRecords() { header[END_TIME_FIELD_NAME] = toHeaderString(&chunk_info.end_time); header[COUNT_FIELD_NAME] = toHeaderString(&chunk_connection_count); - ROS_DEBUG("Writing CHUNK_INFO [%llu]: ver=%d pos=%llu start=%d.%d end=%d.%d", + logDebug("Writing CHUNK_INFO [%llu]: ver=%d pos=%llu start=%d.%d end=%d.%d", (unsigned long long) file_.getOffset(), CHUNK_INFO_VERSION, (unsigned long long) chunk_info.pos, chunk_info.start_time.sec, chunk_info.start_time.nsec, chunk_info.end_time.sec, chunk_info.end_time.nsec); @@ -873,7 +875,7 @@ void Bag::writeChunkInfoRecords() { write((char*) &connection_id, 4); write((char*) &count, 4); - ROS_DEBUG(" - %d: %d", connection_id, count); + logDebug(" - %d: %d", connection_id, count); } } } @@ -902,7 +904,7 @@ void Bag::readChunkInfoRecord() { uint32_t chunk_connection_count = 0; readField(fields, COUNT_FIELD_NAME, true, &chunk_connection_count); - ROS_DEBUG("Read CHUNK_INFO: chunk_pos=%llu connection_count=%d start=%d.%d end=%d.%d", + logDebug("Read CHUNK_INFO: chunk_pos=%llu connection_count=%d start=%d.%d end=%d.%d", (unsigned long long) chunk_info.pos, chunk_connection_count, chunk_info.start_time.sec, chunk_info.start_time.nsec, chunk_info.end_time.sec, chunk_info.end_time.nsec); @@ -913,7 +915,7 @@ void Bag::readChunkInfoRecord() { read((char*) &connection_id, 4); read((char*) &connection_count, 4); - ROS_DEBUG(" %d: %d messages", connection_id, connection_count); + logDebug(" %d: %d messages", connection_id, connection_count); chunk_info.connection_counts[connection_id] = connection_count; } @@ -965,7 +967,7 @@ void Bag::appendDataLengthToBuffer(Buffer& buf, uint32_t data_len) { //! \todo clean this up void Bag::readHeaderFromBuffer(Buffer& buffer, uint32_t offset, ros::Header& header, uint32_t& data_size, uint32_t& bytes_read) const { - ROS_ASSERT(buffer.getSize() > 8); + assert(buffer.getSize() > 8); uint8_t* start = (uint8_t*) buffer.getData() + offset; @@ -994,7 +996,7 @@ void Bag::readMessageDataHeaderFromBuffer(Buffer& buffer, uint32_t offset, ros:: total_bytes_read = 0; uint8_t op = 0xFF; do { - ROS_DEBUG("reading header from buffer: offset=%d", offset); + logDebug("reading header from buffer: offset=%d", offset); uint32_t bytes_read; readHeaderFromBuffer(*current_buffer_, offset, header, data_size, bytes_read); @@ -1074,12 +1076,6 @@ std::string Bag::toHeaderString(Time const* field) const { return toHeaderString(&packed_time); } -ros::AdvertiseOptions createAdvertiseOptions(const ConnectionInfo* c, uint32_t queue_size) { - ros::AdvertiseOptions opts(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; -} // Low-level I/O diff --git a/tools/rosbag_storage/src/bag_player.cpp b/tools/rosbag_storage/src/bag_player.cpp new file mode 100644 index 0000000000..ae87e6a8e4 --- /dev/null +++ b/tools/rosbag_storage/src/bag_player.cpp @@ -0,0 +1,71 @@ +#include "rosbag/bag_player.h" + +#define foreach BOOST_FOREACH + +namespace rosbag +{ + +BagPlayer::BagPlayer(const std::string &fname) throw(BagException) { + 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 topics; + std::pair 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); +} + +} + diff --git a/tools/rosbag/src/buffer.cpp b/tools/rosbag_storage/src/buffer.cpp similarity index 96% rename from tools/rosbag/src/buffer.cpp rename to tools/rosbag_storage/src/buffer.cpp index 679fc08ded..b3d217a8fc 100644 --- a/tools/rosbag/src/buffer.cpp +++ b/tools/rosbag_storage/src/buffer.cpp @@ -32,9 +32,12 @@ * POSSIBILITY OF SUCH DAMAGE. ********************************************************************/ +#include +#include + #include "rosbag/buffer.h" -#include +//#include namespace rosbag { @@ -65,7 +68,7 @@ void Buffer::ensureCapacity(uint32_t capacity) { } buffer_ = (uint8_t*) realloc(buffer_, capacity_); - ROS_ASSERT(buffer_); + assert(buffer_); } } // namespace rosbag diff --git a/tools/rosbag/src/bz2_stream.cpp b/tools/rosbag_storage/src/bz2_stream.cpp similarity index 98% rename from tools/rosbag/src/bz2_stream.cpp rename to tools/rosbag_storage/src/bz2_stream.cpp index 6b38a2f06c..900cd046c2 100644 --- a/tools/rosbag/src/bz2_stream.cpp +++ b/tools/rosbag_storage/src/bz2_stream.cpp @@ -35,8 +35,8 @@ #include "rosbag/chunked_file.h" #include - -#include +#include +#include "console_bridge/console.h" using std::string; @@ -115,7 +115,7 @@ void BZ2Stream::read(void* ptr, size_t size) { case BZ_OK: return; case BZ_STREAM_END: if (getUnused() || getUnusedLength() > 0) - ROS_ERROR("unused data already available"); + logError("unused data already available"); else { char* unused; int nUnused; diff --git a/tools/rosbag/src/chunked_file.cpp b/tools/rosbag_storage/src/chunked_file.cpp similarity index 99% rename from tools/rosbag/src/chunked_file.cpp rename to tools/rosbag_storage/src/chunked_file.cpp index 132acb6112..170afa7cd8 100644 --- a/tools/rosbag/src/chunked_file.cpp +++ b/tools/rosbag_storage/src/chunked_file.cpp @@ -38,7 +38,7 @@ #include -#include +//#include #ifdef _WIN32 # ifdef __MINGW32__ # define fseeko fseeko64 diff --git a/tools/rosbag/src/message_instance.cpp b/tools/rosbag_storage/src/message_instance.cpp similarity index 93% rename from tools/rosbag/src/message_instance.cpp rename to tools/rosbag_storage/src/message_instance.cpp index ff97c82d8c..58702f9296 100644 --- a/tools/rosbag/src/message_instance.cpp +++ b/tools/rosbag_storage/src/message_instance.cpp @@ -27,6 +27,8 @@ #include "rosbag/message_instance.h" +#include "ros/message_event.h" + using std::string; using ros::Time; using boost::shared_ptr; @@ -60,8 +62,4 @@ uint32_t MessageInstance::size() const { return bag_->readMessageDataSize(index_entry_); } -ros::AdvertiseOptions createAdvertiseOptions(MessageInstance const& m, uint32_t queue_size) { - return ros::AdvertiseOptions(m.getTopic(), queue_size, m.getMD5Sum(), m.getDataType(), m.getMessageDefinition()); -} - } // namespace rosbag diff --git a/tools/rosbag/src/query.cpp b/tools/rosbag_storage/src/query.cpp similarity index 100% rename from tools/rosbag/src/query.cpp rename to tools/rosbag_storage/src/query.cpp diff --git a/tools/rosbag/src/stream.cpp b/tools/rosbag_storage/src/stream.cpp similarity index 99% rename from tools/rosbag/src/stream.cpp rename to tools/rosbag_storage/src/stream.cpp index 11cbf13746..b1f52edb26 100644 --- a/tools/rosbag/src/stream.cpp +++ b/tools/rosbag_storage/src/stream.cpp @@ -35,7 +35,7 @@ #include "rosbag/stream.h" #include "rosbag/chunked_file.h" -#include +//#include using boost::shared_ptr; diff --git a/tools/rosbag/src/uncompressed_stream.cpp b/tools/rosbag_storage/src/uncompressed_stream.cpp similarity index 99% rename from tools/rosbag/src/uncompressed_stream.cpp rename to tools/rosbag_storage/src/uncompressed_stream.cpp index fdd810cd92..e0d3e9c57c 100644 --- a/tools/rosbag/src/uncompressed_stream.cpp +++ b/tools/rosbag_storage/src/uncompressed_stream.cpp @@ -35,11 +35,10 @@ #include "rosbag/chunked_file.h" #include +#include #include -#include - using std::string; using boost::format; using ros::Exception; diff --git a/tools/rosbag/src/view.cpp b/tools/rosbag_storage/src/view.cpp similarity index 99% rename from tools/rosbag/src/view.cpp rename to tools/rosbag_storage/src/view.cpp index 27618b064b..7448c861a3 100644 --- a/tools/rosbag/src/view.cpp +++ b/tools/rosbag_storage/src/view.cpp @@ -31,6 +31,7 @@ #include #include +#include #define foreach BOOST_FOREACH @@ -59,7 +60,7 @@ View::iterator::iterator(View* view, bool end) : view_(view), view_revision_(0), View::iterator::iterator(const iterator& i) : view_(i.view_), iters_(i.iters_), view_revision_(i.view_revision_), message_instance_(NULL) { } void View::iterator::populate() { - ROS_ASSERT(view_ != NULL); + assert(view_ != NULL); iters_.clear(); foreach(MessageRange const* range, view_->ranges_) @@ -71,7 +72,7 @@ void View::iterator::populate() { } void View::iterator::populateSeek(multiset::const_iterator iter) { - ROS_ASSERT(view_ != NULL); + assert(view_ != NULL); iters_.clear(); foreach(MessageRange const* range, view_->ranges_) { @@ -101,7 +102,7 @@ bool View::iterator::equal(View::iterator const& other) const { } void View::iterator::increment() { - ROS_ASSERT(view_ != NULL); + assert(view_ != NULL); // Our message instance is no longer valid if (message_instance_ != NULL) diff --git a/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h b/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h index 17f604caf2..b10a21acfc 100644 --- a/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h +++ b/utilities/message_filters/include/message_filters/sync_policies/approximate_time.h @@ -52,6 +52,7 @@ #include #include +#include #include #include diff --git a/utilities/message_filters/include/message_filters/sync_policies/exact_time.h b/utilities/message_filters/include/message_filters/sync_policies/exact_time.h index c1ea6924d0..db33cdca64 100644 --- a/utilities/message_filters/include/message_filters/sync_policies/exact_time.h +++ b/utilities/message_filters/include/message_filters/sync_policies/exact_time.h @@ -52,6 +52,7 @@ #include #include +#include #include #include