Skip to content

Commit

Permalink
ROSBAG SNAPSHOT: address PR comments
Browse files Browse the repository at this point in the history
  • Loading branch information
Kevin Allen committed Jun 7, 2018
1 parent de67d28 commit fda4b36
Show file tree
Hide file tree
Showing 5 changed files with 90 additions and 90 deletions.
2 changes: 1 addition & 1 deletion test/test_rosbag/test/test_snapshot.py
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#!/usr/bin/env python
# Software License Agreement (BSD License)
#
# Copyright (c) 2008, Open Source Robotics Foundation, Inc.
# Copyright (c) 2018, Open Source Robotics Foundation, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
Expand Down
2 changes: 1 addition & 1 deletion tools/rosbag/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@ catkin_package(
add_library(rosbag
src/player.cpp
src/recorder.cpp
src/snapshoter.cpp
src/snapshotter.cpp
src/time_translator.cpp)

add_dependencies(rosbag ${catkin_EXPORTED_TARGETS})
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,13 +51,13 @@

namespace rosbag
{
class ROSBAG_DECL Snapshoter;
class ROSBAG_DECL Snapshotter;

/* Configuration for a single topic in the Snapshoter node. Holds
/* Configuration for a single topic in the Snapshotter node. Holds
* the buffer limits for a topic by duration (time difference between newest and oldest message)
* and memory usage, in bytes.
*/
struct ROSBAG_DECL SnapshoterTopicOptions
struct ROSBAG_DECL SnapshotterTopicOptions
{
// When the value of duration_limit_, do not truncate the buffer no matter how large the duration is
static const ros::Duration NO_DURATION_LIMIT;
Expand All @@ -73,31 +73,31 @@ struct ROSBAG_DECL SnapshoterTopicOptions
// Maximum memory usage of the buffer before older messages ar eremoved
int32_t memory_limit_;

SnapshoterTopicOptions(ros::Duration duration_limit = INHERIT_DURATION_LIMIT,
SnapshotterTopicOptions(ros::Duration duration_limit = INHERIT_DURATION_LIMIT,
int32_t memory_limit = INHERIT_MEMORY_LIMIT);
};

/* Configuration for the Snapshoter node. Contains default limits for memory and duration
/* Configuration for the Snapshotter node. Contains default limits for memory and duration
* and a map of topics to their limits which may override the defaults.
*/
struct ROSBAG_DECL SnapshoterOptions
struct ROSBAG_DECL SnapshotterOptions
{
// Duration limit to use for a topic's buffer if one is not specified
ros::Duration default_duration_limit_;
// Memory limit to use for a topic's buffer if one is not specified
int32_t default_memory_limit_;
// Period between publishing topic status messages. If <= ros::Duration(0), don't publish status
ros::Duration status_period_;
typedef std::map<std::string, SnapshoterTopicOptions> topics_t;
typedef std::map<std::string, SnapshotterTopicOptions> topics_t;
// Provides list of topics to snapshot and their limit configurations
topics_t topics_;

SnapshoterOptions(ros::Duration default_duration_limit = ros::Duration(30), int32_t default_memory_limit = -1,
SnapshotterOptions(ros::Duration default_duration_limit = ros::Duration(30), int32_t default_memory_limit = -1,
ros::Duration status_period = ros::Duration(1));

// Add a new topic to the configuration
void addTopic(std::string const& topic, ros::Duration duration_limit = SnapshoterTopicOptions::INHERIT_DURATION_LIMIT,
int32_t memory_limit = SnapshoterTopicOptions::INHERIT_MEMORY_LIMIT);
void addTopic(std::string const& topic, ros::Duration duration_limit = SnapshotterTopicOptions::INHERIT_DURATION_LIMIT,
int32_t memory_limit = SnapshotterTopicOptions::INHERIT_MEMORY_LIMIT);
};

/* Stores a buffered message of an ambiguous type and it's associated metadata (time of arrival, connection data),
Expand All @@ -119,13 +119,13 @@ struct ROSBAG_DECL SnapshotMessage
*/
class ROSBAG_DECL MessageQueue
{
friend Snapshoter;
friend Snapshotter;

private:
// Locks access to size_ and queue_
boost::mutex lock;
// Stores limits on buffer size and duration
SnapshoterTopicOptions options_;
SnapshotterTopicOptions options_;
// Current total size of the queue, in bytes
int64_t size_;
typedef std::deque<SnapshotMessage> queue_t;
Expand All @@ -134,7 +134,7 @@ class ROSBAG_DECL MessageQueue
boost::shared_ptr<ros::Subscriber> sub_;

public:
MessageQueue(SnapshoterTopicOptions const& options);
MessageQueue(SnapshotterTopicOptions const& options);
// Add a new message to the internal queue if possible, truncating the front of the queue as needed to enforce limits
void push(SnapshotMessage const& msg);
// Removes the message at the front of the queue (oldest) and returns it
Expand Down Expand Up @@ -163,22 +163,22 @@ class ROSBAG_DECL MessageQueue
bool preparePush(int32_t size, ros::Time const& time);
};

/* Snapshoter node. Maintains a circular buffer of the most recent messages from configured topics
/* Snapshotter node. Maintains a circular buffer of the most recent messages from configured topics
* while enforcing limits on memory and duration. The node can be triggered to write some or all
* of these buffers to a bag file via a service call. Useful in live testing scenerios where interesting
* data may be produced before a user has the oppurtunity to "rosbag record" the data.
*/
class ROSBAG_DECL Snapshoter
class ROSBAG_DECL Snapshotter
{
public:
Snapshoter(SnapshoterOptions const& options);
Snapshotter(SnapshotterOptions const& options);
// Sets up callbacks and spins until node is killed
int run();

private:
// Subscribe queue size for each topic
static const int QUEUE_SIZE;
SnapshoterOptions options_;
SnapshotterOptions options_;
typedef std::map<std::string, boost::shared_ptr<MessageQueue> > buffers_t;
buffers_t buffers_;
// Locks recording_ and writing_ states.
Expand All @@ -193,8 +193,8 @@ class ROSBAG_DECL Snapshoter
ros::Publisher status_pub_;
ros::Timer status_timer_;

// Replace individual topic limits with node defaults if they are flagged for it (see SnapshoterTopicOptions)
void fixTopicOptions(SnapshoterTopicOptions& options);
// Replace individual topic limits with node defaults if they are flagged for it (see SnapshotterTopicOptions)
void fixTopicOptions(SnapshotterTopicOptions& options);
// If file is "prefix" mode (doesn't end in .bag), append current datetime and .bag to end
bool postfixFilename(std::string& file);
/// Return current local datetime as a string such as 2018-05-22-14-28-51. Used to generate bag filenames
Expand Down Expand Up @@ -222,17 +222,17 @@ class ROSBAG_DECL Snapshoter
rosbag_msgs::TriggerSnapshot::Request& req, rosbag_msgs::TriggerSnapshot::Response& res);
};

// Configuration for SnapshoterClient
struct ROSBAG_DECL SnapshoterClientOptions
// Configuration for SnapshotterClient
struct ROSBAG_DECL SnapshotterClientOptions
{
SnapshoterClientOptions();
SnapshotterClientOptions();
enum Action
{
TRIGGER_WRITE,
PAUSE,
RESUME
};
// What to do when SnapshoterClient.run is called
// What to do when SnapshotterClient.run is called
Action action_;
// List of topics to write when action_ == TRIGGER_WRITE. If empty, write all buffered topics.
std::vector<std::string> topics_;
Expand All @@ -242,12 +242,12 @@ struct ROSBAG_DECL SnapshoterClientOptions
std::string prefix_;
};

// Node used to call services which interface with the snapshoter node to trigger write, pause, and resume
class ROSBAG_DECL SnapshoterClient
// Node used to call services which interface with the snapshotter node to trigger write, pause, and resume
class ROSBAG_DECL SnapshotterClient
{
public:
SnapshoterClient();
int run(SnapshoterClientOptions const& opts);
SnapshotterClient();
int run(SnapshotterClientOptions const& opts);

private:
ros::NodeHandle nh_;
Expand Down
40 changes: 20 additions & 20 deletions tools/rosbag/src/snapshot.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
********************************************************************/

#include "rosbag/snapshoter.h"
#include "rosbag/snapshotter.h"
#include "rosbag/exceptions.h"

#include "boost/program_options.hpp"
Expand All @@ -41,11 +41,11 @@

namespace po = boost::program_options;

using rosbag::Snapshoter;
using rosbag::SnapshoterClient;
using rosbag::SnapshoterOptions;
using rosbag::SnapshoterTopicOptions;
using rosbag::SnapshoterClientOptions;
using rosbag::Snapshotter;
using rosbag::SnapshotterClient;
using rosbag::SnapshotterOptions;
using rosbag::SnapshotterTopicOptions;
using rosbag::SnapshotterClientOptions;

const int MB_TO_BYTES = 1E6;

Expand Down Expand Up @@ -98,7 +98,7 @@ bool parseOptions(po::variables_map& vm, int argc, char** argv)
return true;
}

bool parseVariablesMap(SnapshoterOptions& opts, po::variables_map const& vm)
bool parseVariablesMap(SnapshotterOptions& opts, po::variables_map const& vm)
{
if (vm.count("topic"))
{
Expand All @@ -111,15 +111,15 @@ bool parseVariablesMap(SnapshoterOptions& opts, po::variables_map const& vm)
return true;
}

bool parseVariablesMapClient(SnapshoterClientOptions& opts, po::variables_map const& vm)
bool parseVariablesMapClient(SnapshotterClientOptions& opts, po::variables_map const& vm)
{
if (vm.count("pause"))
opts.action_ = SnapshoterClientOptions::PAUSE;
opts.action_ = SnapshotterClientOptions::PAUSE;
else if (vm.count("resume"))
opts.action_ = SnapshoterClientOptions::RESUME;
opts.action_ = SnapshotterClientOptions::RESUME;
else if (vm.count("trigger-write"))
{
opts.action_ = SnapshoterClientOptions::TRIGGER_WRITE;
opts.action_ = SnapshotterClientOptions::TRIGGER_WRITE;
if (vm.count("topic"))
opts.topics_ = vm["topic"].as<std::vector<std::string> >();
if (vm.count("output-prefix"))
Expand All @@ -134,7 +134,7 @@ bool parseVariablesMapClient(SnapshoterClientOptions& opts, po::variables_map co
* TODO: use exceptions instead of asserts to follow style conventions
* See snapshot.test in test_rosbag for an example
*/
void appendParamOptions(ros::NodeHandle& nh, SnapshoterOptions& opts)
void appendParamOptions(ros::NodeHandle& nh, SnapshotterOptions& opts)
{
using XmlRpc::XmlRpcValue;
XmlRpcValue topics;
Expand Down Expand Up @@ -169,8 +169,8 @@ void appendParamOptions(ros::NodeHandle& nh, SnapshoterOptions& opts)
ROS_ASSERT_MSG(topic_config.getType() == XmlRpcValue::TypeStruct, "Topic limits invalid for: '%s'",
topic.c_str());

ros::Duration dur = SnapshoterTopicOptions::INHERIT_DURATION_LIMIT;
int64_t mem = SnapshoterTopicOptions::INHERIT_MEMORY_LIMIT;
ros::Duration dur = SnapshotterTopicOptions::INHERIT_DURATION_LIMIT;
int64_t mem = SnapshotterTopicOptions::INHERIT_MEMORY_LIMIT;
std::string duration = "duration";
std::string memory = "memory";
if (topic_config.hasMember(duration))
Expand Down Expand Up @@ -221,16 +221,16 @@ int main(int argc, char** argv)
// If any of the client flags are on, use the client
if (vm.count("trigger-write") || vm.count("pause") || vm.count("resume"))
{
SnapshoterClientOptions opts;
SnapshotterClientOptions opts;
if (!parseVariablesMapClient(opts, vm))
return 1;
ros::init(argc, argv, "snapshot_client", ros::init_options::AnonymousName);
SnapshoterClient client;
SnapshotterClient client;
return client.run(opts);
}

// Parse the command-line options
SnapshoterOptions opts;
SnapshotterOptions opts;
if (!parseVariablesMap(opts, vm))
return 1;

Expand All @@ -246,7 +246,7 @@ int main(int argc, char** argv)
return 1;
}

// Run the snapshoter
rosbag::Snapshoter snapshoter(opts);
return snapshoter.run();
// Run the snapshotter
rosbag::Snapshotter snapshotter(opts);
return snapshotter.run();
}
Loading

0 comments on commit fda4b36

Please sign in to comment.