Skip to content

Commit

Permalink
Comment out portions that require non-merged work
Browse files Browse the repository at this point in the history
Signed-off-by: Emerson Knapp <eknapp@amazon.com>
  • Loading branch information
Emerson Knapp committed Mar 29, 2019
1 parent 1ebe7c0 commit d9f3c4b
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 27 deletions.
16 changes: 14 additions & 2 deletions quality_of_service_demo/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,12 @@ The Publisher in this demo will pause publishing periodically, which will print

Run `quality_of_service_demo/deadline -h` to see the usage for more configuration options.

Examples:
* `deadline 600 -p 5000 -d 2000`
* The publisher will publish for 5 seconds, then pause for 2 - deadline events will start firing on both participants, until the publisher comes back
* `deadline 600 -p 5000 -d 0`
* The publisher doesn't actually pause, no deadline misses should occur

## Lifespan
`quality_of_service_demo/lifespan` demonstrates messages being deleted from a Publisher's outgoing queue once their configured lifespan expires.

Expand All @@ -33,13 +39,19 @@ Examples:
* `lifespan 1000 -s 3000 -p 10`
* After a few seconds, you should see (approximately) messages 4-10 printed from the Subscriber
* The first 3 messages, with 1 second lifespan, were gone by the time the Subscriber joined at 3 seconds
* `lifespan 3000 -s 3000 -p 10`
* `lifespan 4000 -s 3000 -p 10`
* After a few seconds, you should see messages 0-10 printed from the Subscriber
* All messages, with their longer 3 second lifespan, survived until the subscriber joined.
* All messages, with their longer 4 second lifespan, survived until the subscriber joined.

## Liveliness
`quality_of_service_demo/liveliness` demonstrates notification of liveliness changes, based on different Liveliness configurations.

The application requires an argument `lease_duration` that specifies how often (in milliseconds) an entity must "check in" to let the system know it's alive.

The Publisher in this demo will assert its liveliness based on passed in options, and be killed after some amount of time. When the Publisher is killed, you will see a Liveliness change event printed from the Publisher after `lease_duration` expires.

Examples:
* `liveliness 1000 -k 2000`
* After 2 seconds, the publisher will be killed, and the subscriber will receive a callback 1 second after that notifying it that the liveliness has changed
* `liveliness 1000 -n 2000 -p MANUAL_BY_NODE`
* The Subscriber will receive alternating alive/not-alive events every second. The Publisher asserts its liveliness every 2 seconds, but this is not frequent enough for the liveliness lease of 1 second
3 changes: 1 addition & 2 deletions quality_of_service_demo/src/common_nodes.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ Talker::Talker(
max_count_(max_count)
{
RCLCPP_INFO(get_logger(), "Talker starting up");
publisher_ = create_publisher<std_msgs::msg::String>(topic_name, nullptr, pub_options);
publisher_ = create_publisher<std_msgs::msg::String>(topic_name, pub_options);
publish_timer_ = create_wall_timer(
500ms,
[this]() -> void {
Expand Down Expand Up @@ -96,6 +96,5 @@ void Listener::start_listening()
{
RCLCPP_INFO(get_logger(), "Listener heard: [%s]", msg->data.c_str());
},
nullptr,
sub_options_);
}
41 changes: 29 additions & 12 deletions quality_of_service_demo/src/deadline.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,8 @@

#include "quality_of_service_demo/common_nodes.hpp"

static const size_t default_period_pause_talker = 5000;
static const size_t default_duration_pause_talker = 1000;
static const unsigned default_period_pause_talker = 5000;
static const unsigned default_duration_pause_talker = 1000;

void print_usage()
{
Expand All @@ -37,9 +37,9 @@ void print_usage()
printf("deadline_duration: Duration (in ms) of the Deadline QoS setting.\n");
printf("options:\n");
printf("-h : Print this help function.\n");
printf("-p period_pause_talker : How often to pause the talker (in ms). Defaults to %zu.\n",
printf("-p period_pause_talker : How often to pause the talker (in ms). Defaults to %u.\n",
default_period_pause_talker);
printf("-d duration_pause_talker : How long to pause the talker (in ms). Defaults to %zu.\n",
printf("-d duration_pause_talker : How long to pause the talker (in ms). Defaults to %u.\n",
default_duration_pause_talker);
}

Expand Down Expand Up @@ -79,18 +79,35 @@ int main(int argc, char * argv[])
std::string topic("qos_deadline_chatter");

rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
/*
TODO(emersonknapp) once the new types have been added
qos_profile.deadline.sec = deadline_duration_ms / 1000;
qos_profile.deadline.nsec = (deadline_duration_ms % 1000) * 1000000;
printf("Qos prof sec %zu nsec %zu\n", qos_profile.deadline.sec, qos_profile.deadline.nsec);

rclcpp::SubscriptionOptions<> sub_options(qos_profile);
sub_options.deadline_callback(
[](rclcpp::QOSDeadlineRequestedInfo & /* event */) -> void
*/

rclcpp::SubscriptionOptions<> sub_options;
sub_options.qos_profile = qos_profile;
/*
TODO(emersonknapp) once the callbacks have been added
sub_options.event_callbacks.deadline_callback =
[](rclcpp::DeadlineRequestedInfo & event) -> void
{
RCUTILS_LOG_INFO("Requested deadline missed event");
RCUTILS_LOG_INFO("Requested deadline missed - total %d delta %d",
event.total_count, event.total_count_change);
});

rclcpp::PublisherOptions<> pub_options(qos_profile);
*/

rclcpp::PublisherOptions<> pub_options;
pub_options.qos_profile = qos_profile;
/*
TODO(emersonknapp) once the callbacks have been added
pub_options.event_callbacks.deadline_callback =
[](rclcpp::DeadlineOfferedInfo & event) -> void
{
RCUTILS_LOG_INFO("Offered deadline missed - total %d delta %d",
event.total_count, event.total_count_change);
});
*/


auto talker = std::make_shared<Talker>(topic, pub_options);
Expand Down
10 changes: 7 additions & 3 deletions quality_of_service_demo/src/lifespan.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -83,13 +83,17 @@ int main(int argc, char * argv[])

rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
qos_profile.depth = history;
// Need this if we keep
qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
/*
TODO(emersonknapp) once new types are available
qos_profile.lifespan.sec = lifespan_duration_ms / 1000;
qos_profile.lifespan.nsec = (lifespan_duration_ms % 1000) * 1000000;
*/

rclcpp::SubscriptionOptions<> sub_options(qos_profile);
rclcpp::PublisherOptions<> pub_options(qos_profile);
rclcpp::SubscriptionOptions<> sub_options;
sub_options.qos_profile = qos_profile;
rclcpp::PublisherOptions<> pub_options;
pub_options.qos_profile = qos_profile;

auto listener = std::make_shared<Listener>(topic, sub_options, true);
auto talker = std::make_shared<Talker>(topic, pub_options, publish_n_messages);
Expand Down
43 changes: 35 additions & 8 deletions quality_of_service_demo/src/liveliness.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -61,7 +61,8 @@ int main(int argc, char * argv[])
std::chrono::milliseconds node_assert_period(0);
std::chrono::milliseconds topic_assert_period(0);
std::chrono::milliseconds kill_publisher_after(default_kill_publisher_after_ms);
rmw_qos_liveliness_policy_t liveliness_policy_kind = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
// TODO(emersonknapp) once new types are available
// rmw_qos_liveliness_policy_t liveliness_policy_kind = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;

// Optional argument parsing
if (rcutils_cli_option_exist(argv, argv + argc, "-n")) {
Expand All @@ -78,17 +79,20 @@ int main(int argc, char * argv[])
}
if (rcutils_cli_option_exist(argv, argv + argc, "-p")) {
char * policy_str = rcutils_cli_get_option(argv, argv + argc, "-p");
/*
TODO(emersonknapp) once new types are available
if (strcmp(policy_str, "AUTOMATIC") == 0) {
liveliness_policy_kind = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
} else if (strcmp(policy_str, "MANUAL_BY_NODE") == 0) {
liveliness_policy_kind = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_NODE;
} else if (strcmp(policy_str, "MANUAL_BY_TOPIC") == 0) {
liveliness_policy_kind = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
} else {
*/
printf("Unknown liveliness policy: %s\n", policy_str);
print_usage();
return 1;
}
// }
}

// Configuration and Initialization
Expand All @@ -98,18 +102,30 @@ int main(int argc, char * argv[])
std::string topic("qos_liveliness_chatter");

rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
/*
TODO(emersonknapp) once new types are available
qos_profile.liveliness = liveliness_policy_kind;
qos_profile.liveliness_lease_duration.sec = liveliness_lease_duration_ms / 1000;
qos_profile.liveliness_lease_duration.nsec = (liveliness_lease_duration_ms % 1000) * 1000000;

rclcpp::SubscriptionOptions<> sub_options(qos_profile);
sub_options.liveliness_callback(
[](rclcpp::QOSLivelinessChangedInfo & /* event */) -> void
*/

rclcpp::SubscriptionOptions<> sub_options;
sub_options.qos_profile = qos_profile;
/*
TODO(emersonknapp) once callbacks are available
sub_options.event_callbacks.liveliness_callback =
[](rclcpp::QOSLivelinessChangedInfo & event) -> void
{
RCUTILS_LOG_INFO("Liveliness changed event");
printf("Liveliness changed event: \n");
printf(" alive_count: %d\n", event.alive_count);
printf(" not_alive_count: %d\n", event.not_alive_count);
printf(" alive_count_change: %d\n", event.alive_count_change);
printf(" not_alive_count_change: %d\n", event.not_alive_count_change);
});
*/

rclcpp::PublisherOptions<> pub_options(qos_profile);
rclcpp::PublisherOptions<> pub_options;
pub_options.qos_profile = qos_profile;

auto listener = std::make_shared<Listener>(topic, sub_options);
auto talker = std::make_shared<Talker>(
Expand All @@ -125,6 +141,16 @@ int main(int argc, char * argv[])

auto timer = listener->create_wall_timer(
kill_publisher_after,
[talker]() -> void {
if (talker->assert_node_timer_) {
talker->assert_node_timer_->cancel();
}
if (talker->assert_topic_timer_) {
talker->assert_topic_timer_->cancel();
}
});
/*
TODO(emersonknapp) once new types are available
[&exec, talker, liveliness_policy_kind]() -> void {
switch (liveliness_policy_kind) {
case RMW_QOS_POLICY_LIVELINESS_AUTOMATIC:
Expand All @@ -144,6 +170,7 @@ int main(int argc, char * argv[])
break;
}
});
*/

exec.spin();

Expand Down

0 comments on commit d9f3c4b

Please sign in to comment.