Skip to content

Commit

Permalink
feat(default_ad_api): use diagnostic graph (#7043)
Browse files Browse the repository at this point in the history 
Signed-off-by: Takagi, Isamu <[email protected]>
  • Loading branch information
@isamu-takagi
isamu-takagi authored May 24, 2024
1 parent d5e691e commit bf575b6
Show file tree
Hide file tree
Showing 4 changed files with 53 additions and 71 deletions.
74 changes: 38 additions & 36 deletions launch/tier4_system_launch/launch/system.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,11 +25,12 @@
<arg name="run_mode" default="online" description="options: online, logging_simulation, planning_simulation"/>
<arg name="sensor_model" description="sensor model name"/>

<arg name="use_diagnostic_graph" default="false" description="use diagnostic graph packages"/>
<arg name="mrm_handler_param_path" if="$(var use_diagnostic_graph)"/>
<arg name="diagnostic_graph_aggregator_param_path" if="$(var use_diagnostic_graph)"/>
<arg name="diagnostic_graph_aggregator_graph_path" if="$(var use_diagnostic_graph)"/>
<arg name="diagnostic_graph_aggregator_planning_simulator_graph_path" if="$(var use_diagnostic_graph)"/>
<!-- Emergency handler will be replaced by MRM handler. -->
<arg name="use_emergency_handler" default="true" description="use emergency handler packages"/>
<arg name="mrm_handler_param_path"/>
<arg name="diagnostic_graph_aggregator_param_path"/>
<arg name="diagnostic_graph_aggregator_graph_path_main"/>
<arg name="diagnostic_graph_aggregator_graph_path_psim"/>

<let name="sensor_launch_pkg" value="$(find-pkg-share $(var sensor_model)_launch)"/>

Expand Down Expand Up @@ -57,6 +58,13 @@
</include>
</group>

<!-- Duplicated Node Checker -->
<group>
<include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
<arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
</include>
</group>

<!-- Service Log Checker -->
<group>
<include file="$(find-pkg-share component_interface_tools)/launch/service_log_checker.launch.xml"/>
Expand All @@ -70,8 +78,20 @@
</include>
</group>

<!-- MRM Operator -->
<group>
<include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
<arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
</include>
</group>
<group>
<include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
<arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
</include>
</group>

<!-- Error Monitor -->
<group unless="$(var use_diagnostic_graph)">
<group if="$(var use_emergency_handler)">
<let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
<let name="config_file" value="$(var system_error_monitor_param_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
<let name="config_file" value="$(var system_error_monitor_planning_simulator_param_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
Expand All @@ -85,52 +105,34 @@
</group>

<!-- Emergency Handler -->
<group unless="$(var use_diagnostic_graph)">
<group if="$(var use_emergency_handler)">
<include file="$(find-pkg-share emergency_handler)/launch/emergency_handler.launch.xml">
<arg name="config_file" value="$(var emergency_handler_param_path)"/>
</include>
</group>

<!-- Diagnostic Graph Aggregator -->
<group>
<include file="$(find-pkg-share duplicated_node_checker)/launch/duplicated_node_checker.launch.xml">
<arg name="config_file" value="$(var duplicated_node_checker_param_path)"/>
<let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_main)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
<let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_main)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
<let name="diagnostic_graph_aggregator_graph_path" value="$(var diagnostic_graph_aggregator_graph_path_psim)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
<include file="$(find-pkg-share system_diagnostic_monitor)/launch/system_diagnostic_monitor.launch.xml">
<arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
<arg name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)"/>
</include>
</group>

<!-- MRM Operator -->
<group>
<include file="$(find-pkg-share mrm_comfortable_stop_operator)/launch/mrm_comfortable_stop_operator.launch.py">
<arg name="config_file" value="$(var mrm_comfortable_stop_operator_param_path)"/>
</include>
</group>
<group>
<include file="$(find-pkg-share mrm_emergency_stop_operator)/launch/mrm_emergency_stop_operator.launch.py">
<arg name="config_file" value="$(var mrm_emergency_stop_operator_param_path)"/>
</include>
<!-- Hazard Status Converter -->
<group unless="$(var use_emergency_handler)">
<include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
</group>

<!-- MRM Handler -->
<group if="$(var use_diagnostic_graph)">
<group unless="$(var use_emergency_handler)">
<include file="$(find-pkg-share mrm_handler)/launch/mrm_handler.launch.xml">
<arg name="config_file" value="$(var mrm_handler_param_path)"/>
</include>
</group>

<!-- Diagnostic Graph Aggregator -->
<group if="$(var use_diagnostic_graph)">
<let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='online'&quot;)"/>
<let name="graph_file" value="$(var diagnostic_graph_aggregator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='logging_simulation'&quot;)"/>
<let name="graph_file" value="$(var diagnostic_graph_aggregator_planning_simulator_graph_path)" if="$(eval &quot;'$(var run_mode)'=='planning_simulation'&quot;)"/>
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
<arg name="param_file" value="$(var diagnostic_graph_aggregator_param_path)"/>
<arg name="graph_file" value="$(var graph_file)"/>
</include>
</group>

<!-- Hazard Status Converter -->
<group if="$(var use_diagnostic_graph)">
<include file="$(find-pkg-share hazard_status_converter)/launch/hazard_status_converter.launch.xml"/>
</group>
</group>

<!-- Dummy Diag Publisher -->
Expand Down
40 changes: 10 additions & 30 deletions system/default_ad_api/src/operation_mode.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,18 +39,15 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options)
adaptor.init_cli(cli_mode_, group_cli_);
adaptor.init_cli(cli_control_, group_cli_);

const std::vector<std::string> module_names = {
"sensing", "perception", "map", "localization", "planning", "control", "vehicle", "system",
const auto name = "/system/operation_mode/availability";
const auto qos = rclcpp::QoS(1);
const auto callback = [this](const OperationModeAvailability::ConstSharedPtr msg) {
mode_available_[OperationModeState::Message::STOP] = msg->stop;
mode_available_[OperationModeState::Message::AUTONOMOUS] = msg->autonomous;
mode_available_[OperationModeState::Message::LOCAL] = msg->local;
mode_available_[OperationModeState::Message::REMOTE] = msg->remote;
};

for (size_t i = 0; i < module_names.size(); ++i) {
const auto name = "/system/component_state_monitor/component/autonomous/" + module_names[i];
const auto qos = rclcpp::QoS(1).transient_local();
const auto callback = [this, i, module_names](const ModeChangeAvailable::ConstSharedPtr msg) {
module_states_[module_names[i]] = msg->available;
};
sub_module_states_.push_back(create_subscription<ModeChangeAvailable>(name, qos, callback));
}
sub_availability_ = create_subscription<OperationModeAvailability>(name, qos, callback);

timer_ = rclcpp::create_timer(
this, get_clock(), rclcpp::Rate(5.0).period(), std::bind(&OperationModeNode::on_timer, this));
Expand All @@ -60,8 +57,8 @@ OperationModeNode::OperationModeNode(const rclcpp::NodeOptions & options)
mode_available_[OperationModeState::Message::UNKNOWN] = false;
mode_available_[OperationModeState::Message::STOP] = true;
mode_available_[OperationModeState::Message::AUTONOMOUS] = false;
mode_available_[OperationModeState::Message::LOCAL] = true;
mode_available_[OperationModeState::Message::REMOTE] = true;
mode_available_[OperationModeState::Message::LOCAL] = false;
mode_available_[OperationModeState::Message::REMOTE] = false;
}

template <class ResponseT>
Expand Down Expand Up @@ -135,23 +132,6 @@ void OperationModeNode::on_state(const OperationModeState::Message::ConstSharedP

void OperationModeNode::on_timer()
{
bool autonomous_available = true;
std::string unhealthy_components = "";
for (const auto & state : module_states_) {
if (!state.second) {
unhealthy_components += unhealthy_components.empty() ? state.first : ", " + state.first;
}
autonomous_available &= state.second;
}
mode_available_[OperationModeState::Message::AUTONOMOUS] = autonomous_available;

if (!unhealthy_components.empty()) {
RCLCPP_INFO_THROTTLE(
get_logger(), *get_clock(), 3000,
"%s component state is unhealthy. Autonomous is not available.",
unhealthy_components.c_str());
}

update_state();
}

Expand Down
8 changes: 3 additions & 5 deletions system/default_ad_api/src/operation_mode.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,7 @@
#include <vector>

// TODO(Takagi, Isamu): define interface
#include <tier4_system_msgs/msg/mode_change_available.hpp>
#include <tier4_system_msgs/msg/operation_mode_availability.hpp>

// This file should be included after messages.
#include "utils/types.hpp"
Expand All @@ -47,7 +47,7 @@ class OperationModeNode : public rclcpp::Node
using ChangeToRemote = autoware_ad_api::operation_mode::ChangeToRemote;
using OperationModeRequest = system_interface::ChangeOperationMode::Service::Request;
using AutowareControlRequest = system_interface::ChangeAutowareControl::Service::Request;
using ModeChangeAvailable = tier4_system_msgs::msg::ModeChangeAvailable;
using OperationModeAvailability = tier4_system_msgs::msg::OperationModeAvailability;

OperationModeState::Message curr_state_;
OperationModeState::Message prev_state_;
Expand All @@ -65,9 +65,7 @@ class OperationModeNode : public rclcpp::Node
Sub<system_interface::OperationModeState> sub_state_;
Cli<system_interface::ChangeOperationMode> cli_mode_;
Cli<system_interface::ChangeAutowareControl> cli_control_;

std::unordered_map<std::string, bool> module_states_;
std::vector<rclcpp::Subscription<ModeChangeAvailable>::SharedPtr> sub_module_states_;
rclcpp::Subscription<OperationModeAvailability>::SharedPtr sub_availability_;

void on_change_to_stop(
const ChangeToStop::Service::Request::SharedPtr req,
Expand Down
2 changes: 2 additions & 0 deletions system/system_diagnostic_monitor/launch/system_diagnostic_monitor.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,8 @@
<launch>
<arg name="param_file" default="$(find-pkg-share diagnostic_graph_aggregator)/config/default.param.yaml"/>
<arg name="graph_file" default="$(find-pkg-share system_diagnostic_monitor)/config/autoware-main.yaml"/>
<include file="$(find-pkg-share diagnostic_graph_aggregator)/launch/aggregator.launch.xml">
<arg name="param_file" value="$(var param_file)"/>
<arg name="graph_file" value="$(var graph_file)"/>
</include>
<node pkg="system_diagnostic_monitor" exec="component_state_diagnostics" name="component_state_diagnostics"/>
Expand Down

0 comments on commit bf575b6

Please sign in to comment.