Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(topic_state_monitor): support transform topic check #1586

Merged
merged 3 commits into from
Sep 20, 2022
Merged

feat(topic_state_monitor): support transform topic check #1586

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d882a43
feat(topic_state_monitor): support transform topic check
isamu-takagi Aug 15, 2022
f547006
Merge branch 'main' into feature/topic-state-monitor-tf
isamu-takagi Sep 20, 2022
7496e41
Merge branch 'main' into feature/topic-state-monitor-tf
isamu-takagi Sep 20, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 16 additions & 14 deletions system/topic_state_monitor/Readme.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,23 +35,25 @@ The types of topic status and corresponding diagnostic status are following.

### Node Parameters

| Name | Type | Default Value | Description |
| ------------- | ------ | ------------- | ----------------------------------------------------- |
| `update_rate` | double | 10.0 | Timer callback period [Hz] |
| `window_size` | int | 10 | Window size of target topic for calculating frequency |
| Name | Type | Default Value | Description |
| ----------------- | ------ | ------------- | ------------------------------------------------------------- |
| `topic` | string | - | Name of target topic |
| `topic_type` | string | - | Type of target topic (used if the topic is not transform) |
| `frame_id` | string | - | Frame ID of transform parent (used if the topic is transform) |
| `child_frame_id` | string | - | Frame ID of transform child (used if the topic is transform) |
| `transient_local` | bool | false | QoS policy of topic subscription (Transient Local/Volatile) |
| `best_effort` | bool | false | QoS policy of topic subscription (Best Effort/Reliable) |
| `diag_name` | string | - | Name used for the diagnostics to publish |
| `update_rate` | double | 10.0 | Timer callback period [Hz] |

### Core Parameters

| Name | Type | Default Value | Description |
| ----------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------- |
| `topic` | string | - | Name of target topic |
| `topic_type` | string | - | Type of target topic |
| `transient_local` | bool | false | QoS policy of topic subscription (Transient Local/Volatile) |
| `best_effort` | bool | false | QoS policy of topic subscription (Best Effort/Reliable) |
| `diag_name` | string | - | Name used for the diagnostics to publish |
| `warn_rate` | double | 0.5 | If the topic rate is lower than this value, the topic status becomes `WarnRate` |
| `error_rate` | double | 0.1 | If the topic rate is lower than this value, the topic status becomes `ErrorRate` |
| `timeout` | double | 1.0 | If the topic subscription is stopped for more than this time [s], the topic status becomes `Timeout` |
| Name | Type | Default Value | Description |
| ------------- | ------ | ------------- | ---------------------------------------------------------------------------------------------------- |
| `warn_rate` | double | 0.5 | If the topic rate is lower than this value, the topic status becomes `WarnRate` |
| `error_rate` | double | 0.1 | If the topic rate is lower than this value, the topic status becomes `ErrorRate` |
| `timeout` | double | 1.0 | If the topic subscription is stopped for more than this time [s], the topic status becomes `Timeout` |
| `window_size` | int | 10 | Window size of target topic for calculating frequency |

## Assumptions / Known limits

kenji-miyake marked this conversation as resolved.
Show resolved Hide resolved
Expand Down
5 changes: 0 additions & 5 deletions system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,6 @@ namespace topic_state_monitor
{
struct Param
{
std::string topic;
std::string topic_type;
bool transient_local;
bool best_effort;
std::string diag_name;
double warn_rate;
double error_rate;
double timeout;
Expand Down
11 changes: 11 additions & 0 deletions system/topic_state_monitor/include/topic_state_monitor/topic_state_monitor_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,8 @@
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>

#include <tf2_msgs/msg/tf_message.hpp>

#include <deque>
#include <map>
#include <memory>
Expand All @@ -31,6 +33,14 @@ namespace topic_state_monitor
struct NodeParam
{
double update_rate;
std::string diag_name;
std::string topic;
std::string topic_type;
std::string frame_id;
std::string child_frame_id;
bool transient_local;
bool best_effort;
bool is_transform;
};

class TopicStateMonitorNode : public rclcpp::Node
Expand All @@ -53,6 +63,7 @@ class TopicStateMonitorNode : public rclcpp::Node

// Subscriber
rclcpp::GenericSubscription::SharedPtr sub_topic_;
rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr sub_transform_;

// Timer
void onTimer();
Expand Down
26 changes: 26 additions & 0 deletions system/topic_state_monitor/launch/topic_state_monitor_tf.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,26 @@
<launch>
<arg name="node_name_suffix" description="node name suffix"/>
<arg name="topic" description="transform topic name"/>
<arg name="frame_id" description="parent frame id"/>
<arg name="child_frame_id" description="child frame id"/>
<arg name="transient_local" default="false" description="add transient_local option to subscriber or not"/>
<arg name="best_effort" default="false" description="add best_effort option to subscriber or not"/>
<arg name="diag_name" description="diag name"/>
<arg name="warn_rate" description="warn rate[Hz]"/>
<arg name="error_rate" description="error rate[Hz]"/>
<arg name="timeout" description="timeout period[s]"/>
<arg name="window_size" default="10" description="window size"/>

<node pkg="topic_state_monitor" exec="topic_state_monitor_node" name="topic_state_monitor_$(var node_name_suffix)" output="screen">
<param name="topic" value="$(var topic)"/>
<param name="frame_id" value="$(var frame_id)"/>
<param name="child_frame_id" value="$(var child_frame_id)"/>
<param name="transient_local" value="$(var transient_local)"/>
<param name="best_effort" value="$(var best_effort)"/>
<param name="diag_name" value="$(var diag_name)"/>
<param name="warn_rate" value="$(var warn_rate)"/>
<param name="error_rate" value="$(var error_rate)"/>
<param name="timeout" value="$(var timeout)"/>
<param name="window_size" value="$(var window_size)"/>
</node>
</launch>
1 change: 1 addition & 0 deletions system/topic_state_monitor/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
<depend>diagnostic_updater</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tf2_msgs</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
57 changes: 43 additions & 14 deletions system/topic_state_monitor/src/topic_state_monitor_core.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,13 +40,22 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op
: Node("topic_state_monitor", node_options), updater_(this)
{
using std::placeholders::_1;

// Parameter
node_param_.update_rate = declare_parameter("update_rate", 10.0);
param_.topic = declare_parameter<std::string>("topic");
param_.topic_type = declare_parameter<std::string>("topic_type");
param_.transient_local = declare_parameter("transient_local", false);
param_.best_effort = declare_parameter("best_effort", false);
param_.diag_name = declare_parameter<std::string>("diag_name");
node_param_.topic = declare_parameter<std::string>("topic");
node_param_.transient_local = declare_parameter("transient_local", false);
node_param_.best_effort = declare_parameter("best_effort", false);
node_param_.diag_name = declare_parameter<std::string>("diag_name");
node_param_.is_transform = (node_param_.topic == "/tf" || node_param_.topic == "/tf_static");

if (node_param_.is_transform) {
node_param_.frame_id = declare_parameter<std::string>("frame_id");
node_param_.child_frame_id = declare_parameter<std::string>("child_frame_id");
} else {
node_param_.topic_type = declare_parameter<std::string>("topic_type");
}

param_.warn_rate = declare_parameter("warn_rate", 0.5);
param_.error_rate = declare_parameter("error_rate", 0.1);
param_.timeout = declare_parameter("timeout", 1.0);
Expand All @@ -62,21 +71,35 @@ TopicStateMonitorNode::TopicStateMonitorNode(const rclcpp::NodeOptions & node_op

// Subscriber
rclcpp::QoS qos = rclcpp::QoS{1};
if (param_.transient_local) {
if (node_param_.transient_local) {
qos.transient_local();
}
if (param_.best_effort) {
if (node_param_.best_effort) {
qos.best_effort();
}
sub_topic_ = this->create_generic_subscription(
param_.topic, param_.topic_type, qos,
[this]([[maybe_unused]] std::shared_ptr<rclcpp::SerializedMessage> msg) {
topic_state_monitor_->update();
});

if (node_param_.is_transform) {
sub_transform_ = this->create_subscription<tf2_msgs::msg::TFMessage>(
node_param_.topic, qos, [this](tf2_msgs::msg::TFMessage::ConstSharedPtr msg) {
for (const auto & transform : msg->transforms) {
if (
transform.header.frame_id == node_param_.frame_id &&
transform.child_frame_id == node_param_.child_frame_id) {
topic_state_monitor_->update();
}
}
});
} else {
sub_topic_ = this->create_generic_subscription(
node_param_.topic, node_param_.topic_type, qos,
[this]([[maybe_unused]] std::shared_ptr<rclcpp::SerializedMessage> msg) {
topic_state_monitor_->update();
});
}

// Diagnostic Updater
updater_.setHardwareID("topic_state_monitor");
updater_.add(param_.diag_name, this, &TopicStateMonitorNode::checkTopicStatus);
updater_.add(node_param_.diag_name, this, &TopicStateMonitorNode::checkTopicStatus);

// Timer
const auto period_ns = rclcpp::Rate(node_param_.update_rate).period();
Expand All @@ -96,6 +119,7 @@ rcl_interfaces::msg::SetParametersResult TopicStateMonitorNode::onParameter(
update_param(parameters, "error_rate", param_.error_rate);
update_param(parameters, "timeout", param_.timeout);
update_param(parameters, "window_size", param_.window_size);
topic_state_monitor_->setParam(param_);
} catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
result.successful = false;
result.reason = e.what();
Expand All @@ -120,7 +144,12 @@ void TopicStateMonitorNode::checkTopicStatus(diagnostic_updater::DiagnosticStatu
const auto topic_rate = topic_state_monitor_->getTopicRate();

// Add topic name
stat.addf("topic", "%s", param_.topic.c_str());
if (node_param_.is_transform) {
const auto frame = "(" + node_param_.frame_id + " to " + node_param_.child_frame_id + ")";
stat.addf("topic", "%s %s", node_param_.topic.c_str(), frame.c_str());
} else {
stat.addf("topic", "%s", node_param_.topic.c_str());
}

// Judge level
int8_t level = DiagnosticStatus::OK;
Expand Down