Skip to content

Commit

Permalink
Address review comments
Browse files Browse the repository at this point in the history
Signed-off-by: Devin Bonnie <dbbonnie@amazon.com>
  • Loading branch information
dabonnie committed Apr 20, 2020
1 parent d879e56 commit 7bd867e
Show file tree
Hide file tree
Showing 3 changed files with 70 additions and 41 deletions.
8 changes: 4 additions & 4 deletions rclcpp/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -541,9 +541,9 @@ if(BUILD_TESTING)
target_link_libraries(test_wait_set ${PROJECT_NAME})
endif()

ament_add_gtest(test_subscriber_topic_statistics test/topic_statistics/test_subscriber_topic_statistics.cpp)
if(TARGET test_subscriber_topic_statistics)
ament_target_dependencies(test_subscriber_topic_statistics
ament_add_gtest(test_subscription_topic_statistics test/topic_statistics/test_subscription_topic_statistics.cpp)
if(TARGET test_subscription_topic_statistics)
ament_target_dependencies(test_subscription_topic_statistics
"libstatistics_collector"
"rcl_interfaces"
"rcutils"
Expand All @@ -552,7 +552,7 @@ if(BUILD_TESTING)
"rosidl_typesupport_cpp"
"statistics_msgs"
"test_msgs")
target_link_libraries(test_subscriber_topic_statistics ${PROJECT_NAME})
target_link_libraries(test_subscription_topic_statistics ${PROJECT_NAME})
endif()

# Install test resources
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_
#define RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_
#ifndef RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_
#define RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_

#include <memory>
#include <string>
Expand All @@ -37,8 +37,8 @@ namespace rclcpp
namespace topic_statistics
{

constexpr const char kDefaultPublishTopicName[]{"topic_statistics"};
constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::minutes(1)};
constexpr const char kDefaultPublishTopicName[]{"/statistics"};
constexpr const std::chrono::milliseconds kDefaultPublishingPeriod{std::chrono::seconds(1)};

using libstatistics_collector::collector::GenerateStatisticMessage;
using statistics_msgs::msg::MetricsMessage;
Expand All @@ -51,7 +51,7 @@ using libstatistics_collector::moving_average_statistics::StatisticData;
* \tparam CallbackMessageT the subscribed message type
*/
template<typename CallbackMessageT>
class SubscriberTopicStatistics
class SubscriptionTopicStatistics
{
using TopicStatsCollector =
libstatistics_collector::topic_statistics_collector::TopicStatisticsCollector<
Expand All @@ -61,28 +61,33 @@ class SubscriberTopicStatistics
CallbackMessageT>;

public:
/// Construct a SubscriberTopicStatistics object.
/// Construct a SubscriptionTopicStatistics object.
/**
* This object wraps utilities, defined in libstatistics_collector, to collect,
* measure, and publish topic statistics data.
* measure, and publish topic statistics data. This throws an invalid_argument
* if the input publisher is null.
*
* @param node_name the name of the node, which created this instance, in order to denote
* \param node_name the name of the node, which created this instance, in order to denote
* topic source
* @param publisher instance constructed by the node in order to publish statistics data.
* This class owns the publisher/
* \param publisher instance constructed by the node in order to publish statistics data.
* This class owns the publisher.
*/
SubscriberTopicStatistics(
SubscriptionTopicStatistics(
const std::string & node_name,
rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher)
: node_name_(node_name),
publisher_(std::move(publisher))
{
// TODO(dbbonnie): ros-tooling/aws-roadmap/issues/226, received message age
// TODO(dbbonnie) throw if publisher is null

if (nullptr == publisher_) {
throw std::invalid_argument("publisher pointer is nullptr");
}

bring_up();
}

virtual ~SubscriberTopicStatistics()
virtual ~SubscriptionTopicStatistics()
{
tear_down();
}
Expand All @@ -101,29 +106,16 @@ class SubscriberTopicStatistics
}
}

/// Return a vector of all the currently collected data
/**
* \return a vector of all the collected data
*/
std::vector<StatisticData> get_current_collector_data() const
{
std::vector<StatisticData> data;
for (const auto & collector : subscriber_statistics_collectors_) {
data.push_back(collector->GetStatisticsResults());
}
return data;
}

/// Set the timer used to publish statistics messages.
/**
* @param measurement_timer the timer to fire the publisher, created by the node
* \param measurement_timer the timer to fire the publisher, created by the node
*/
void set_publisher_timer(const rclcpp::TimerBase::SharedPtr & publisher_timer)
void set_publisher_timer(rclcpp::TimerBase::SharedPtr publisher_timer)
{
publisher_timer_ = publisher_timer;
}

/// Publish a populated MetricsStatisticsMessage
/// Publish a populated MetricsStatisticsMessage.
virtual void publish_message()
{
rclcpp::Time window_end{get_current_nanoseconds_since_epoch()};
Expand All @@ -143,6 +135,20 @@ class SubscriberTopicStatistics
window_start_ = window_end;
}

protected:
/// Return a vector of all the currently collected data.
/**
* \return a vector of all the collected data
*/
std::vector<StatisticData> get_current_collector_data() const
{
std::vector<StatisticData> data;
for (const auto & collector : subscriber_statistics_collectors_) {
data.push_back(collector->GetStatisticsResults());
}
return data;
}

private:
/// Construct and start all collectors and set window_start_.
void bring_up()
Expand Down Expand Up @@ -175,7 +181,7 @@ class SubscriberTopicStatistics
/**
* \return the current nanoseconds (count) since epoch
*/
int64_t get_current_nanoseconds_since_epoch()
int64_t get_current_nanoseconds_since_epoch() const
{
const auto now = std::chrono::system_clock::now();
return std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
Expand All @@ -184,7 +190,7 @@ class SubscriberTopicStatistics
/// Collection of statistics collectors
std::vector<std::unique_ptr<TopicStatsCollector>> subscriber_statistics_collectors_{};
/// Node name used to generate topic statistics messages to be published
std::string node_name_;
const std::string node_name_;
/// Publisher, created by the node, used to publish topic statistics messages
rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher_;
/// Timer which fires the publisher
Expand All @@ -195,4 +201,4 @@ class SubscriberTopicStatistics
} // namespace topic_statistics
} // namespace rclcpp

#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIBER_TOPIC_STATISTICS_HPP_
#endif // RCLCPP__TOPIC_STATISTICS__SUBSCRIPTION_TOPIC_STATISTICS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@

#include <memory>
#include <string>
#include <vector>

#include "libstatistics_collector/moving_average_statistics/types.hpp"

Expand All @@ -24,7 +25,7 @@
#include "rclcpp/qos.hpp"
#include "rclcpp/rclcpp.hpp"

#include "rclcpp/topic_statistics/subscriber_topic_statistics.hpp"
#include "rclcpp/topic_statistics/subscription_topic_statistics.hpp"

#include "statistics_msgs/msg/metrics_message.hpp"
#include "test_msgs/msg/empty.hpp"
Expand All @@ -39,9 +40,29 @@ constexpr const uint64_t kNoSamples{0};

using test_msgs::msg::Empty;
using statistics_msgs::msg::MetricsMessage;
using rclcpp::topic_statistics::SubscriberTopicStatistics;
using rclcpp::topic_statistics::SubscriptionTopicStatistics;
using libstatistics_collector::moving_average_statistics::StatisticData;

template<typename CallbackMessageT>
class TestSubscriptionTopicStatistics : public SubscriptionTopicStatistics<CallbackMessageT>
{
public:
TestSubscriptionTopicStatistics(
const std::string & node_name,
rclcpp::Publisher<statistics_msgs::msg::MetricsMessage>::SharedPtr publisher)
: SubscriptionTopicStatistics<CallbackMessageT>(node_name, publisher)
{
}

virtual ~TestSubscriptionTopicStatistics() = default;

/// Exposed for testing
std::vector<StatisticData> get_current_collector_data() const
{
return SubscriptionTopicStatistics<CallbackMessageT>::get_current_collector_data();
}
};

/**
* Empty subscriber node: used to create subscriber topic statistics requirements
*/
Expand All @@ -61,6 +82,8 @@ class EmptySubscriber : public rclcpp::Node
callback);
}

virtual ~EmptySubscriber() = default;

private:
void receive_message(const Empty &) const
{
Expand All @@ -72,7 +95,7 @@ class EmptySubscriber : public rclcpp::Node
/**
* Test fixture to bring up and teardown rclcpp
*/
class TestSubscriberTopicStatisticsFixture : public ::testing::Test
class TestSubscriptionTopicStatisticsFixture : public ::testing::Test
{
protected:
void SetUp()
Expand All @@ -91,7 +114,7 @@ class TestSubscriberTopicStatisticsFixture : public ::testing::Test
std::shared_ptr<EmptySubscriber> empty_subscriber;
};

TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction)
TEST_F(TestSubscriptionTopicStatisticsFixture, test_manual_construction)
{
// manually create publisher tied to the node
auto topic_stats_publisher =
Expand All @@ -100,7 +123,7 @@ TEST_F(TestSubscriberTopicStatisticsFixture, test_manual_construction)
10);

// construct the instance
auto sub_topic_stats = std::make_unique<SubscriberTopicStatistics<Empty>>(
auto sub_topic_stats = std::make_unique<TestSubscriptionTopicStatistics<Empty>>(
empty_subscriber->get_name(),
topic_stats_publisher);

Expand Down

0 comments on commit 7bd867e

Please sign in to comment.