diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index a134fd775155b..d13e628f2e1d4 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -497,8 +497,13 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist; const bool departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist; - const bool keep_stopped_condition = - m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged; + // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity + static constexpr double vel_epsilon = 1e-3; + + // Let vehicle start after the steering is converged for control accuracy + const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon && + m_enable_keep_stopped_until_steer_convergence && + !lateral_sync_data_.is_steer_converged; const bool stopping_condition = stop_dist < p.stopping_state_stop_dist; @@ -528,8 +533,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time : false; - static constexpr double vel_epsilon = - 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity const double current_vel_cmd = std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps); const bool emergency_condition = m_enable_overshoot_emergency && diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 58a8e7e181e35..f2c6782b3ef6e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -222,6 +222,23 @@ void NormalLaneChange::insertStopPoint( const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; + const auto is_valid_start_point = std::invoke([&]() -> bool { + auto lc_start_point = lanelet::utils::conversion::toLaneletPoint( + status_.lane_change_path.info.lane_changing_start.position); + const auto target_neighbor_preferred_lane_poly_2d = + utils::lane_change::getTargetNeighborLanesPolygon( + *route_handler, status_.current_lanes, type_); + return boost::geometry::covered_by( + lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d); + }); + + if (!is_valid_start_point) { + const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); + + return; + } + // calculate minimum distance from path front to the stationary object on the ego lane. const auto distance_to_ego_lane_obj = [&]() -> double { double distance_to_obj = distance_to_terminal; @@ -646,7 +663,8 @@ double NormalLaneChange::calcMaximumLaneChangeLength( const auto shift_intervals = getRouteHandler()->getLateralIntervalsToPreferredLane(current_terminal_lanelet); return utils::lane_change::calcMaximumLaneChangeLength( - getEgoVelocity(), getCommonParam(), shift_intervals, max_acc); + std::max(getCommonParam().minimum_lane_changing_velocity, getEgoVelocity()), getCommonParam(), + shift_intervals, max_acc); } std::vector NormalLaneChange::sampleLongitudinalAccValues( @@ -1250,6 +1268,20 @@ bool NormalLaneChange::getLaneChangePaths( boost::geometry::covered_by(lc_start_point, target_neighbor_preferred_lane_poly_2d) || boost::geometry::covered_by(lc_start_point, target_lane_poly_2d); + LaneChangeInfo lane_change_info; + lane_change_info.longitudinal_acceleration = + LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; + lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; + lane_change_info.velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; + lane_change_info.current_lanes = current_lanes; + lane_change_info.target_lanes = target_lanes; + lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; + lane_change_info.lane_changing_end = target_segment.points.front().point.pose; + lane_change_info.lateral_acceleration = lateral_acc; + lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; + if (!is_valid_start_point) { debug_print( "Reject: lane changing points are not inside of the target preferred lanes or its " @@ -1269,21 +1301,8 @@ bool NormalLaneChange::getLaneChangePaths( continue; } - LaneChangeInfo lane_change_info; - lane_change_info.longitudinal_acceleration = - LaneChangePhaseInfo{longitudinal_acc_on_prepare, longitudinal_acc_on_lane_changing}; - lane_change_info.duration = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; - lane_change_info.velocity = - LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; - lane_change_info.length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; - lane_change_info.current_lanes = current_lanes; - lane_change_info.target_lanes = target_lanes; - lane_change_info.lane_changing_start = prepare_segment.points.back().point.pose; - lane_change_info.lane_changing_end = target_segment.points.front().point.pose; lane_change_info.shift_line = utils::lane_change::getLaneChangingShiftLine( prepare_segment, target_segment, target_lane_reference_path, shift_length); - lane_change_info.lateral_acceleration = lateral_acc; - lane_change_info.terminal_lane_changing_velocity = terminal_lane_changing_velocity; const auto candidate_path = utils::lane_change::constructCandidatePath( lane_change_info, prepare_segment, target_segment, target_lane_reference_path, diff --git a/planning/planning_validator/include/planning_validator/planning_validator.hpp b/planning/planning_validator/include/planning_validator/planning_validator.hpp index ef570b57773bb..037c7fb0f4e95 100644 --- a/planning/planning_validator/include/planning_validator/planning_validator.hpp +++ b/planning/planning_validator/include/planning_validator/planning_validator.hpp @@ -109,7 +109,7 @@ class PlanningValidator : public rclcpp::Node int diag_error_count_threshold_ = 0; bool display_on_terminal_ = true; - Updater diag_updater_{this}; + std::shared_ptr diag_updater_ = nullptr; PlanningValidatorStatus validation_status_; ValidationParams validation_params_; // for thresholds diff --git a/planning/planning_validator/src/planning_validator.cpp b/planning/planning_validator/src/planning_validator.cpp index 875781d47b25d..58af2c08ccb22 100644 --- a/planning/planning_validator/src/planning_validator.cpp +++ b/planning/planning_validator/src/planning_validator.cpp @@ -47,10 +47,6 @@ PlanningValidator::PlanningValidator(const rclcpp::NodeOptions & options) setupParameters(); - if (publish_diag_) { - setupDiag(); - } - logger_configure_ = std::make_unique(this); } @@ -112,39 +108,40 @@ void PlanningValidator::setStatus( void PlanningValidator::setupDiag() { + diag_updater_ = std::make_shared(this); auto & d = diag_updater_; - d.setHardwareID("planning_validator"); + d->setHardwareID("planning_validator"); std::string ns = "trajectory_validation_"; - d.add(ns + "finite", [&](auto & stat) { + d->add(ns + "finite", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_finite_value, "infinite value is found"); }); - d.add(ns + "interval", [&](auto & stat) { + d->add(ns + "interval", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_interval, "points interval is too long"); }); - d.add(ns + "relative_angle", [&](auto & stat) { + d->add(ns + "relative_angle", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_relative_angle, "relative angle is too large"); }); - d.add(ns + "curvature", [&](auto & stat) { + d->add(ns + "curvature", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_curvature, "curvature is too large"); }); - d.add(ns + "lateral_acceleration", [&](auto & stat) { + d->add(ns + "lateral_acceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_lateral_acc, "lateral acceleration is too large"); }); - d.add(ns + "acceleration", [&](auto & stat) { + d->add(ns + "acceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_longitudinal_max_acc, "acceleration is too large"); }); - d.add(ns + "deceleration", [&](auto & stat) { + d->add(ns + "deceleration", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_longitudinal_min_acc, "deceleration is too large"); }); - d.add(ns + "steering", [&](auto & stat) { + d->add(ns + "steering", [&](auto & stat) { setStatus(stat, validation_status_.is_valid_steering, "expected steering is too large"); }); - d.add(ns + "steering_rate", [&](auto & stat) { + d->add(ns + "steering_rate", [&](auto & stat) { setStatus( stat, validation_status_.is_valid_steering_rate, "expected steering rate is too large"); }); - d.add(ns + "velocity_deviation", [&](auto & stat) { + d->add(ns + "velocity_deviation", [&](auto & stat) { setStatus( stat, validation_status_.is_valid_velocity_deviation, "velocity deviation is too large"); }); @@ -174,11 +171,15 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg) if (!isDataReady()) return; + if (publish_diag_ && !diag_updater_) { + setupDiag(); // run setup after all data is ready. + } + debug_pose_publisher_->clearMarkers(); validate(*current_trajectory_); - diag_updater_.force_update(); + diag_updater_->force_update(); publishTrajectory(); diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 4b25dba8b16ee..b0df04285ac9f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -171,36 +171,36 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node tf2_ros::TransformListener tf_listener_; /* received & published topics */ - PoseWithCovarianceStamped::ConstSharedPtr initial_pose_; - TwistStamped initial_twist_; - VelocityReport current_velocity_; - Odometry current_odometry_; - SteeringReport current_steer_; - AckermannControlCommand current_ackermann_cmd_; - AckermannControlCommand current_manual_ackermann_cmd_; - GearCommand current_gear_cmd_; - GearCommand current_manual_gear_cmd_; - TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_; - HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_; - Trajectory::ConstSharedPtr current_trajectory_ptr_; - bool simulate_motion_; //!< stop vehicle motion simulation if false - ControlModeReport current_control_mode_; - bool enable_road_slope_simulation_; + PoseWithCovarianceStamped::ConstSharedPtr initial_pose_{}; + TwistStamped initial_twist_{}; + VelocityReport current_velocity_{}; + Odometry current_odometry_{}; + SteeringReport current_steer_{}; + AckermannControlCommand current_ackermann_cmd_{}; + AckermannControlCommand current_manual_ackermann_cmd_{}; + GearCommand current_gear_cmd_{}; + GearCommand current_manual_gear_cmd_{}; + TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_{}; + HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_{}; + Trajectory::ConstSharedPtr current_trajectory_ptr_{}; + bool simulate_motion_ = true; //!< stop vehicle motion simulation if false + ControlModeReport current_control_mode_{}; + bool enable_road_slope_simulation_ = true; /* frame_id */ - std::string simulated_frame_id_; //!< @brief simulated vehicle frame id - std::string origin_frame_id_; //!< @brief map frame_id + std::string simulated_frame_id_ = ""; //!< @brief simulated vehicle frame id + std::string origin_frame_id_ = ""; //!< @brief map frame_id /* flags */ - bool is_initialized_; //!< @brief flag to check the initial position is set - bool add_measurement_noise_; //!< @brief flag to add measurement noise + bool is_initialized_ = false; //!< @brief flag to check the initial position is set + bool add_measurement_noise_ = false; //!< @brief flag to add measurement noise - DeltaTime delta_time_; //!< @brief to calculate delta time + DeltaTime delta_time_{}; //!< @brief to calculate delta time - MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise + MeasurementNoiseGenerator measurement_noise_{}; //!< @brief for measurement noise - double x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate - double y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate + double x_stddev_ = 0.0; //!< @brief x standard deviation for dummy covariance in map coordinate + double y_stddev_ = 0.0; //!< @brief y standard deviation for dummy covariance in map coordinate /* vehicle model */ enum class VehicleModelType { diff --git a/system/system_diagnostic_graph/CMakeLists.txt b/system/system_diagnostic_graph/CMakeLists.txt index 81d8ba39b3d1b..142aa94eeef69 100644 --- a/system/system_diagnostic_graph/CMakeLists.txt +++ b/system/system_diagnostic_graph/CMakeLists.txt @@ -4,13 +4,15 @@ project(system_diagnostic_graph) find_package(autoware_cmake REQUIRED) autoware_package() -ament_auto_add_executable(aggregator +ament_auto_add_library(${PROJECT_NAME} SHARED src/core/config.cpp src/core/debug.cpp src/core/graph.cpp - src/core/nodes.cpp - src/core/exprs.cpp + src/core/units.cpp src/core/modes.cpp +) + +ament_auto_add_executable(aggregator src/main.cpp ) @@ -18,9 +20,11 @@ ament_auto_add_executable(converter src/tool.cpp ) -install( - PROGRAMS util/debug.py - DESTINATION lib/${PROJECT_NAME} -) +if(BUILD_TESTING) + get_filename_component(RESOURCE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/test/files ABSOLUTE) + ament_auto_add_gtest(gtest_${PROJECT_NAME} test/src/test.cpp) + target_compile_definitions(gtest_${PROJECT_NAME} PRIVATE TEST_RESOURCE_PATH="${RESOURCE_PATH}") + target_include_directories(gtest_${PROJECT_NAME} PRIVATE src) +endif() ament_auto_package(INSTALL_TO_SHARE config example launch) diff --git a/system/system_diagnostic_graph/example/example_0.yaml b/system/system_diagnostic_graph/example/example_0.yaml index 0ee6e59c8e121..fc4436bba1595 100644 --- a/system/system_diagnostic_graph/example/example_0.yaml +++ b/system/system_diagnostic_graph/example/example_0.yaml @@ -1,32 +1,37 @@ files: - - { package: system_diagnostic_graph, path: example/example_1.yaml } - - { package: system_diagnostic_graph, path: example/example_2.yaml } + - { path: $(find-pkg-share system_diagnostic_graph)/example/example_1.yaml } + - { path: $(find-pkg-share system_diagnostic_graph)/example/example_2.yaml } nodes: - path: /autoware/modes/stop - type: debug-ok + type: ok - path: /autoware/modes/autonomous type: and list: - - { type: link, path: /functions/pose_estimation } - - { type: link, path: /functions/obstacle_detection } + - { type: link, link: /functions/pose_estimation } + - { type: link, link: /functions/obstacle_detection } - path: /autoware/modes/local type: and list: - - { type: link, path: /external/joystick_command } + - { type: link, link: /external/joystick_command } - path: /autoware/modes/remote type: and list: - - { type: link, path: /external/remote_command } + - { type: link, link: /external/remote_command } - path: /autoware/modes/emergency-stop - type: debug-ok + type: ok - path: /autoware/modes/comfortable-stop - type: debug-ok + type: and + list: + - { type: link, link: /functions/obstacle_detection } - path: /autoware/modes/pull-over - type: debug-ok + type: and + list: + - { type: link, link: /functions/pose_estimation } + - { type: link, link: /functions/obstacle_detection } diff --git a/system/system_diagnostic_graph/example/example_1.yaml b/system/system_diagnostic_graph/example/example_1.yaml index 5ba93ec3059e4..07d4951b89446 100644 --- a/system/system_diagnostic_graph/example/example_1.yaml +++ b/system/system_diagnostic_graph/example/example_1.yaml @@ -2,22 +2,22 @@ nodes: - path: /functions/pose_estimation type: and list: - - { type: link, path: /sensing/lidars/top } + - { type: link, link: /sensing/lidars/top } - path: /functions/obstacle_detection type: or list: - - { type: link, path: /sensing/lidars/front } - - { type: link, path: /sensing/radars/front } + - { type: link, link: /sensing/lidars/front } + - { type: link, link: /sensing/radars/front } - path: /sensing/lidars/top type: diag - name: "lidar_driver/top: status" + diag: "lidar_driver/top: status" - path: /sensing/lidars/front type: diag - name: "lidar_driver/front: status" + diag: "lidar_driver/front: status" - path: /sensing/radars/front type: diag - name: "radar_driver/front: status" + diag: "radar_driver/front: status" diff --git a/system/system_diagnostic_graph/example/example_2.yaml b/system/system_diagnostic_graph/example/example_2.yaml index f61f4accbfec8..a03701b661ba9 100644 --- a/system/system_diagnostic_graph/example/example_2.yaml +++ b/system/system_diagnostic_graph/example/example_2.yaml @@ -1,8 +1,8 @@ nodes: - path: /external/joystick_command type: diag - name: "external_command_checker: joystick_command" + diag: "external_command_checker: joystick_command" - path: /external/remote_command type: diag - name: "external_command_checker: remote_command" + diag: "external_command_checker: remote_command" diff --git a/system/system_diagnostic_graph/package.xml b/system/system_diagnostic_graph/package.xml index ffc0c6c7d5385..7855edcde9c62 100644 --- a/system/system_diagnostic_graph/package.xml +++ b/system/system_diagnostic_graph/package.xml @@ -15,6 +15,7 @@ tier4_system_msgs yaml_cpp_vendor + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/system/system_diagnostic_graph/src/core/config.cpp b/system/system_diagnostic_graph/src/core/config.cpp index 305860af32840..2339e96f3951f 100644 --- a/system/system_diagnostic_graph/src/core/config.cpp +++ b/system/system_diagnostic_graph/src/core/config.cpp @@ -14,11 +14,15 @@ #include "config.hpp" +#include "error.hpp" + #include #include #include +#include #include +#include #include // DEBUG @@ -27,235 +31,265 @@ namespace system_diagnostic_graph { -ErrorMarker::ErrorMarker(const std::string & file) +template +void extend(std::vector & u, const std::vector & v) { - file_ = file; + u.insert(u.end(), v.begin(), v.end()); } -std::string ErrorMarker::str() const +template +auto enumerate(const std::vector & v) { - if (type_.empty()) { - return file_; - } - - std::string result = type_; - for (const auto & index : indices_) { - result += "-" + std::to_string(index); + std::vector> result; + for (size_t i = 0; i < v.size(); ++i) { + result.push_back(std::make_pair(i, v[i])); } - return result + " in " + file_; -} - -ErrorMarker ErrorMarker::type(const std::string & type) const -{ - ErrorMarker mark = *this; - mark.type_ = type; - return mark; + return result; } -ErrorMarker ErrorMarker::index(size_t index) const +ConfigData::ConfigData(const std::string & path) { - ErrorMarker mark = *this; - mark.indices_.push_back(index); - return mark; + file = path; } -ConfigObject::ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type) +ConfigData ConfigData::load(YAML::Node yaml) { if (!yaml.IsMap()) { - throw create_error(mark, type + " is not a dict type"); + throw error("object is not a dict type", *this); } for (const auto & kv : yaml) { - dict_[kv.first.as()] = kv.second; + object[kv.first.as()] = kv.second; } - mark_ = mark; - type_ = type; + return *this; } -ErrorMarker ConfigObject::mark() const +ConfigData ConfigData::type(const std::string & name) const { - return mark_; + ConfigData data(file); + data.mark = name; + return data; } -std::optional ConfigObject::take_yaml(const std::string & name) +ConfigData ConfigData::node(const size_t index) const { - if (!dict_.count(name)) { - return std::nullopt; - } - const auto yaml = dict_.at(name); - dict_.erase(name); - return yaml; + ConfigData data(file); + data.mark = mark + "-" + std::to_string(index); + return data; } -std::string ConfigObject::take_text(const std::string & name) +std::string ConfigData::take_text(const std::string & name) { - if (!dict_.count(name)) { - throw create_error(mark_, "object has no '" + name + "' field"); + if (!object.count(name)) { + throw error("required field is not found", name, *this); } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); return yaml.as(); } -std::string ConfigObject::take_text(const std::string & name, const std::string & fail) +std::string ConfigData::take_text(const std::string & name, const std::string & fail) { - if (!dict_.count(name)) { + if (!object.count(name)) { return fail; } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); return yaml.as(); } -std::vector ConfigObject::take_list(const std::string & name) +std::vector ConfigData::take_list(const std::string & name) { - if (!dict_.count(name)) { + if (!object.count(name)) { return std::vector(); } - const auto yaml = dict_.at(name); - dict_.erase(name); + const auto yaml = object.at(name); + object.erase(name); if (!yaml.IsSequence()) { - throw ConfigError("the '" + name + "' field is not a list type"); + throw error("field is not a list type", name, *this); } return std::vector(yaml.begin(), yaml.end()); } -bool ConfigFilter::check(const std::string & mode) const +void check_config_nodes(const std::vector & nodes) { - if (!excludes.empty() && excludes.count(mode) != 0) return false; - if (!includes.empty() && includes.count(mode) == 0) return false; - return true; -} + std::unordered_map path_count; + for (const auto & node : nodes) { + path_count[node->path] += 1; + } -ConfigError create_error(const ErrorMarker & mark, const std::string & message) -{ - (void)mark; - return ConfigError(message); + path_count.erase(""); + for (const auto & [path, count] : path_count) { + if (1 < count) { + throw error("object path is not unique", path); + } + } } -ConfigFilter parse_mode_filter(const ErrorMarker & mark, std::optional yaml) +void resolve_link_nodes(std::vector & nodes) { - std::unordered_set excludes; - std::unordered_set includes; - if (yaml) { - ConfigObject dict(mark, yaml.value(), "mode filter"); + std::vector filtered; + std::unordered_map links; + std::unordered_map paths; - for (const auto & mode : dict.take_list("except")) { - excludes.emplace(mode.as()); + for (const auto & node : nodes) { + links[node] = node; + paths[node->path] = node; + } + + for (const auto & node : nodes) { + if (node->type == "link" && node->path == "") { + const auto link = node->data.take_text("link"); + if (!paths.count(link)) { + throw error("link path is not found", link, node->data); + } + links[node] = paths.at(link); + } else { + filtered.push_back(node); } - for (const auto & mode : dict.take_list("only")) { - includes.emplace(mode.as()); + } + nodes = filtered; + + for (const auto & node : nodes) { + for (auto & child : node->children) { + child = links.at(child); } } - return ConfigFilter{excludes, includes}; } -FileConfig parse_file_config(const ErrorMarker & mark, YAML::Node yaml) +std::string complement_node_type(ConfigData & data) { - ConfigObject dict(mark, yaml, "file object"); - const auto relative_path = dict.take_text("path"); - const auto package_name = dict.take_text("package"); - const auto package_path = ament_index_cpp::get_package_share_directory(package_name); - return FileConfig{mark, package_path + "/" + relative_path}; + if (data.object.count("diag")) { + return "diag"; + } + return data.take_text("type"); } -NodeConfig parse_node_config(const ErrorMarker & mark, YAML::Node yaml) +std::string resolve_substitution(const std::string & substitution, const ConfigData & data) { - ConfigObject dict(mark, yaml, "node object"); - const auto path = dict.take_text("path"); - const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); - return NodeConfig{path, mode, dict}; + std::stringstream ss(substitution); + std::string word; + std::vector words; + while (getline(ss, word, ' ')) { + words.push_back(word); + } + + if (words.size() == 2 && words[0] == "find-pkg-share") { + return ament_index_cpp::get_package_share_directory(words[1]); + } + if (words.size() == 1 && words[0] == "dirname") { + return std::filesystem::path(data.file).parent_path(); + } + throw error("unknown substitution", substitution, data); } -ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml) +std::string resolve_file_path(const std::string & path, const ConfigData & data) { - ConfigObject dict(mark, yaml, "expr object"); - return parse_expr_config(dict); + static const std::regex pattern(R"(\$\(([^()]*)\))"); + std::smatch m; + std::string result = path; + while (std::regex_search(result, m, pattern)) { + const std::string prefix = m.prefix(); + const std::string suffix = m.suffix(); + result = prefix + resolve_substitution(m.str(1), data) + suffix; + } + return result; } -ExprConfig parse_expr_config(ConfigObject & dict) +PathConfig::SharedPtr parse_path_config(const ConfigData & data) { - const auto type = dict.take_text("type"); - const auto mode = parse_mode_filter(dict.mark(), dict.take_yaml("mode")); - return ExprConfig{type, mode, dict}; + const auto path = std::make_shared(data); + path->original = path->data.take_text("path"); + path->resolved = resolve_file_path(path->original, path->data); + return path; } -void dump(const ConfigFile & config) +UnitConfig::SharedPtr parse_node_config(const ConfigData & data) { - std::cout << "=================================================================" << std::endl; - std::cout << config.mark.str() << std::endl; - for (const auto & file : config.files) { - std::cout << " - f: " << file.path << " (" << file.mark.str() << ")" << std::endl; - } - for (const auto & unit : config.units) { - std::cout << " - u: " << unit.path << " (" << unit.dict.mark().str() << ")" << std::endl; + const auto node = std::make_shared(data); + node->path = node->data.take_text("path", ""); + node->type = node->data.take_text("type", ""); + + if (node->type.empty()) { + node->type = complement_node_type(node->data); } - for (const auto & diag : config.diags) { - std::cout << " - d: " << diag.path << " (" << diag.dict.mark().str() << ")" << std::endl; + + for (const auto & [index, yaml] : enumerate(node->data.take_list("list"))) { + const auto child = data.node(index).load(yaml); + node->children.push_back(parse_node_config(child)); } + return node; } -template -auto apply(const ErrorMarker & mark, F & func, const std::vector & list) +FileConfig::SharedPtr parse_file_config(const ConfigData & data) { - std::vector result; - for (size_t i = 0; i < list.size(); ++i) { - result.push_back(func(mark.index(i), list[i])); + const auto file = std::make_shared(data); + const auto path_data = data.type("file"); + const auto node_data = data.type("node"); + const auto paths = file->data.take_list("files"); + const auto nodes = file->data.take_list("nodes"); + + for (const auto & [index, yaml] : enumerate(paths)) { + const auto path = path_data.node(index).load(yaml); + file->paths.push_back(parse_path_config(path)); } - return result; + for (const auto & [index, yaml] : enumerate(nodes)) { + const auto node = node_data.node(index).load(yaml); + file->nodes.push_back(parse_node_config(node)); + } + return file; } -ConfigFile load_config_file(const FileConfig & file) +FileConfig::SharedPtr load_file_config(PathConfig & config) { - if (!std::filesystem::exists(file.path)) { - throw create_error(file.mark, "config file '" + file.path + "' does not exist"); + const auto path = std::filesystem::path(config.resolved); + if (!std::filesystem::exists(path)) { + throw error("file is not found", path, config.data); } + const auto file = ConfigData(path).load(YAML::LoadFile(path)); + return parse_file_config(file); +} - const auto yaml = YAML::LoadFile(file.path); - const auto mark = ErrorMarker(file.path); - auto dict = ConfigObject(mark, yaml, "config file"); +RootConfig load_root_config(const PathConfig::SharedPtr root) +{ + std::vector paths; + paths.push_back(root); + + std::vector files; + for (size_t i = 0; i < paths.size(); ++i) { + const auto path = paths[i]; + const auto file = load_file_config(*path); + files.push_back(file); + extend(paths, file->paths); + } - std::vector units; - std::vector diags; - for (const auto & node : dict.take_list("nodes")) { - const auto type = node["type"].as(); - if (type == "diag") { - diags.push_back(node); - } else { - units.push_back(node); - } + std::vector nodes; + for (const auto & file : files) { + extend(nodes, file->nodes); } + for (size_t i = 0; i < nodes.size(); ++i) { + const auto node = nodes[i]; + extend(nodes, node->children); + } + + check_config_nodes(nodes); + resolve_link_nodes(nodes); - ConfigFile config(mark); - config.files = apply(mark.type("file"), parse_file_config, dict.take_list("files")); - config.units = apply(mark.type("unit"), parse_node_config, units); - config.diags = apply(mark.type("diag"), parse_node_config, diags); + RootConfig config; + config.files = files; + config.nodes = nodes; return config; } -ConfigFile load_config_root(const std::string & path) +RootConfig load_root_config(const std::string & path) { - const auto mark = ErrorMarker("root file"); - std::vector configs; - configs.push_back(load_config_file(FileConfig{mark, path})); - - // Use an index because updating the vector invalidates the iterator. - for (size_t i = 0; i < configs.size(); ++i) { - for (const auto & file : configs[i].files) { - configs.push_back(load_config_file(file)); - } - } - - ConfigFile result(mark); - for (const auto & config : configs) { - result.files.insert(result.files.end(), config.files.begin(), config.files.end()); - result.units.insert(result.units.end(), config.units.begin(), config.units.end()); - result.diags.insert(result.diags.end(), config.diags.begin(), config.diags.end()); - } - return result; + const auto root = std::make_shared(ConfigData("root-file")); + root->original = path; + root->resolved = path; + return load_root_config(root); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/config.hpp b/system/system_diagnostic_graph/src/core/config.hpp index 4d679ef575944..d959f5b6be8aa 100644 --- a/system/system_diagnostic_graph/src/core/config.hpp +++ b/system/system_diagnostic_graph/src/core/config.hpp @@ -18,94 +18,92 @@ #include #include -#include -#include #include #include -#include #include namespace system_diagnostic_graph { -struct ConfigError : public std::runtime_error +struct ConfigData { - using runtime_error::runtime_error; -}; - -class ErrorMarker -{ -public: - explicit ErrorMarker(const std::string & file = ""); - std::string str() const; - ErrorMarker type(const std::string & type) const; - ErrorMarker index(size_t index) const; - -private: - std::string file_; - std::string type_; - std::vector indices_; -}; + explicit ConfigData(const std::string & file); + ConfigData load(YAML::Node yaml); + ConfigData type(const std::string & name) const; + ConfigData node(const size_t index) const; -class ConfigObject -{ -public: - ConfigObject(const ErrorMarker & mark, YAML::Node yaml, const std::string & type); - ErrorMarker mark() const; - std::optional take_yaml(const std::string & name); std::string take_text(const std::string & name); std::string take_text(const std::string & name, const std::string & fail); std::vector take_list(const std::string & name); -private: - ErrorMarker mark_; - std::string type_; - std::unordered_map dict_; + std::string file; + std::string mark; + std::unordered_map object; }; -struct ConfigFilter +struct BaseConfig { - bool check(const std::string & mode) const; - std::unordered_set excludes; - std::unordered_set includes; + explicit BaseConfig(const ConfigData & data) : data(data) {} + ConfigData data; }; -struct ExprConfig +struct PathConfig : public BaseConfig { - std::string type; - ConfigFilter mode; - ConfigObject dict; + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::string original; + std::string resolved; }; -struct NodeConfig +struct UnitConfig : public BaseConfig { + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::string type; std::string path; - ConfigFilter mode; - ConfigObject dict; + std::vector children; }; -struct FileConfig +struct FileConfig : public BaseConfig { - ErrorMarker mark; - std::string path; + using SharedPtr = std::shared_ptr; + using BaseConfig::BaseConfig; + std::vector paths; + std::vector nodes; }; -struct ConfigFile +struct RootConfig { - explicit ConfigFile(const ErrorMarker & mark) : mark(mark) {} - ErrorMarker mark; - std::vector files; - std::vector units; - std::vector diags; + std::vector files; + std::vector nodes; }; -using ConfigDict = std::unordered_map; +template +T error(const std::string & text, const ConfigData & data) +{ + const auto hint = data.mark.empty() ? data.file : data.mark + ":" + data.file; + return T(text + " (" + hint + ")"); +} + +template +T error(const std::string & text) +{ + return T(text); +} -ConfigError create_error(const ErrorMarker & mark, const std::string & message); -ConfigFile load_config_root(const std::string & path); +template +T error(const std::string & text, const std::string & value, const ConfigData & data) +{ + return error(text + ": " + value, data); +} + +template +T error(const std::string & text, const std::string & value) +{ + return error(text + ": " + value); +} -ExprConfig parse_expr_config(const ErrorMarker & mark, YAML::Node yaml); -ExprConfig parse_expr_config(ConfigObject & dict); +RootConfig load_root_config(const std::string & path); } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/debug.cpp b/system/system_diagnostic_graph/src/core/debug.cpp index ed696573521a7..f14177f4571ad 100644 --- a/system/system_diagnostic_graph/src/core/debug.cpp +++ b/system/system_diagnostic_graph/src/core/debug.cpp @@ -15,8 +15,8 @@ #include "debug.hpp" #include "graph.hpp" -#include "nodes.hpp" #include "types.hpp" +#include "units.hpp" #include #include @@ -36,7 +36,9 @@ void Graph::debug() { std::vector lines; for (const auto & node : nodes_) { - lines.push_back(node->debug()); + const auto level_name = level_names.at(node->level()); + const auto index_name = std::to_string(node->index()); + lines.push_back({"unit", index_name, level_name, node->path(), "-----"}); } std::array widths = {}; @@ -57,25 +59,4 @@ void Graph::debug() } } -DiagDebugData UnitNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"unit", index_name, level_name, path_, "-----"}; -} - -DiagDebugData DiagNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"diag", index_name, level_name, path_, name_}; -} - -DiagDebugData UnknownNode::debug() const -{ - const auto level_name = level_names.at(level()); - const auto index_name = std::to_string(index()); - return {"test", index_name, level_name, path_, "-----"}; -} - } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/error.hpp b/system/system_diagnostic_graph/src/core/error.hpp new file mode 100644 index 0000000000000..2c10d659f2df4 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/error.hpp @@ -0,0 +1,65 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CORE__ERROR_HPP_ +#define CORE__ERROR_HPP_ + +#include + +namespace system_diagnostic_graph +{ + +struct Exception : public std::runtime_error +{ + using runtime_error::runtime_error; +}; + +class FileNotFound : public Exception +{ + using Exception::Exception; +}; + +class UnknownType : public Exception +{ + using Exception::Exception; +}; + +class InvalidType : public Exception +{ + using Exception::Exception; +}; + +class FieldNotFound : public Exception +{ + using Exception::Exception; +}; + +class PathConflict : public Exception +{ + using Exception::Exception; +}; + +class PathNotFound : public Exception +{ + using Exception::Exception; +}; + +class GraphStructure : public Exception +{ + using Exception::Exception; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__ERROR_HPP_ diff --git a/system/system_diagnostic_graph/src/core/exprs.cpp b/system/system_diagnostic_graph/src/core/exprs.cpp deleted file mode 100644 index 22281f939cad2..0000000000000 --- a/system/system_diagnostic_graph/src/core/exprs.cpp +++ /dev/null @@ -1,216 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "exprs.hpp" - -#include "nodes.hpp" - -#include -#include -#include -#include -#include - -// DEBUG -#include - -namespace system_diagnostic_graph -{ - -using LinkStatus = std::vector>; - -void extend(LinkStatus & a, const LinkStatus & b) -{ - a.insert(a.end(), b.begin(), b.end()); -} - -void extend_false(LinkStatus & a, const LinkStatus & b) -{ - for (const auto & p : b) { - a.push_back(std::make_pair(p.first, false)); - } -} - -auto create_expr_list(ExprInit & exprs, ConfigObject & config) -{ - std::vector> result; - const auto list = config.take_list("list"); - for (size_t i = 0; i < list.size(); ++i) { - auto dict = parse_expr_config(config.mark().index(i), list[i]); - auto expr = exprs.create(dict); - if (expr) { - result.push_back(std::move(expr)); - } - } - return result; -} - -ConstExpr::ConstExpr(const DiagnosticLevel level) -{ - level_ = level; -} - -ExprStatus ConstExpr::eval() const -{ - ExprStatus status; - status.level = level_; - return status; -} - -std::vector ConstExpr::get_dependency() const -{ - return {}; -} - -LinkExpr::LinkExpr(ExprInit & exprs, ConfigObject & config) -{ - // TODO(Takagi, Isamu): remove - (void)config; - (void)exprs; -} - -void LinkExpr::init(ConfigObject & config, std::unordered_map nodes) -{ - const auto path = config.take_text("path"); - if (!nodes.count(path)) { - throw ConfigError("node path '" + path + "' does not exist"); - } - node_ = nodes.at(path); -} - -ExprStatus LinkExpr::eval() const -{ - ExprStatus status; - status.level = node_->level(); - status.links.push_back(std::make_pair(node_, true)); - return status; -} - -std::vector LinkExpr::get_dependency() const -{ - return {node_}; -} - -AndExpr::AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit) -{ - list_ = create_expr_list(exprs, config); - short_circuit_ = short_circuit; -} - -ExprStatus AndExpr::eval() const -{ - if (list_.empty()) { - ExprStatus status; - status.level = DiagnosticStatus::OK; - return status; - } - - ExprStatus status; - status.level = DiagnosticStatus::OK; - for (const auto & expr : list_) { - const auto result = expr->eval(); - status.level = std::max(status.level, result.level); - extend(status.links, result.links); - if (short_circuit_ && status.level != DiagnosticStatus::OK) { - break; - } - } - status.level = std::min(status.level, DiagnosticStatus::ERROR); - return status; -} - -std::vector AndExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -OrExpr::OrExpr(ExprInit & exprs, ConfigObject & config) -{ - list_ = create_expr_list(exprs, config); -} - -ExprStatus OrExpr::eval() const -{ - if (list_.empty()) { - ExprStatus status; - status.level = DiagnosticStatus::OK; - return status; - } - - ExprStatus status; - status.level = DiagnosticStatus::ERROR; - for (const auto & expr : list_) { - const auto result = expr->eval(); - status.level = std::min(status.level, result.level); - extend(status.links, result.links); - } - status.level = std::min(status.level, DiagnosticStatus::ERROR); - return status; -} - -std::vector OrExpr::get_dependency() const -{ - std::vector depends; - for (const auto & expr : list_) { - const auto nodes = expr->get_dependency(); - depends.insert(depends.end(), nodes.begin(), nodes.end()); - } - return depends; -} - -ExprInit::ExprInit(const std::string & mode) -{ - mode_ = mode; -} - -std::unique_ptr ExprInit::create(ExprConfig config) -{ - if (!config.mode.check(mode_)) { - return nullptr; - } - if (config.type == "link") { - auto expr = std::make_unique(*this, config.dict); - links_.push_back(std::make_pair(expr.get(), config.dict)); - return expr; - } - if (config.type == "and") { - return std::make_unique(*this, config.dict, false); - } - if (config.type == "short-circuit-and") { - return std::make_unique(*this, config.dict, true); - } - if (config.type == "or") { - return std::make_unique(*this, config.dict); - } - if (config.type == "debug-ok") { - return std::make_unique(DiagnosticStatus::OK); - } - if (config.type == "debug-warn") { - return std::make_unique(DiagnosticStatus::WARN); - } - if (config.type == "debug-error") { - return std::make_unique(DiagnosticStatus::ERROR); - } - if (config.type == "debug-stale") { - return std::make_unique(DiagnosticStatus::STALE); - } - throw ConfigError("unknown expr type: " + config.type + " " + config.dict.mark().str()); -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/exprs.hpp b/system/system_diagnostic_graph/src/core/exprs.hpp deleted file mode 100644 index f582e019d5691..0000000000000 --- a/system/system_diagnostic_graph/src/core/exprs.hpp +++ /dev/null @@ -1,104 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CORE__EXPRS_HPP_ -#define CORE__EXPRS_HPP_ - -#include "config.hpp" -#include "types.hpp" - -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -struct ExprStatus -{ - DiagnosticLevel level; - std::vector> links; -}; - -class BaseExpr -{ -public: - virtual ~BaseExpr() = default; - virtual ExprStatus eval() const = 0; - virtual std::vector get_dependency() const = 0; -}; - -class ConstExpr : public BaseExpr -{ -public: - explicit ConstExpr(const DiagnosticLevel level); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - DiagnosticLevel level_; -}; - -class LinkExpr : public BaseExpr -{ -public: - LinkExpr(ExprInit & exprs, ConfigObject & config); - void init(ConfigObject & config, std::unordered_map nodes); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - BaseNode * node_; -}; - -class AndExpr : public BaseExpr -{ -public: - AndExpr(ExprInit & exprs, ConfigObject & config, bool short_circuit); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - bool short_circuit_; - std::vector> list_; -}; - -class OrExpr : public BaseExpr -{ -public: - OrExpr(ExprInit & exprs, ConfigObject & config); - ExprStatus eval() const override; - std::vector get_dependency() const override; - -private: - std::vector> list_; -}; - -class ExprInit -{ -public: - explicit ExprInit(const std::string & mode); - std::unique_ptr create(ExprConfig config); - auto get() const { return links_; } - -private: - std::string mode_; - std::vector> links_; -}; - -} // namespace system_diagnostic_graph - -#endif // CORE__EXPRS_HPP_ diff --git a/system/system_diagnostic_graph/src/core/graph.cpp b/system/system_diagnostic_graph/src/core/graph.cpp index b4fd1d15827f3..96e9bcff5bfd9 100644 --- a/system/system_diagnostic_graph/src/core/graph.cpp +++ b/system/system_diagnostic_graph/src/core/graph.cpp @@ -15,8 +15,8 @@ #include "graph.hpp" #include "config.hpp" -#include "exprs.hpp" -#include "nodes.hpp" +#include "error.hpp" +#include "units.hpp" #include #include @@ -31,12 +31,12 @@ namespace system_diagnostic_graph { -void topological_sort(std::vector> & input) +BaseUnit::UniquePtrList topological_sort(BaseUnit::UniquePtrList && input) { - std::unordered_map degrees; - std::deque nodes; - std::deque result; - std::deque buffer; + std::unordered_map degrees; + std::deque nodes; + std::deque result; + std::deque buffer; // Create a list of raw pointer nodes. for (const auto & node : input) { @@ -45,8 +45,8 @@ void topological_sort(std::vector> & input) // Count degrees of each node. for (const auto & node : nodes) { - for (const auto & link : node->links()) { - ++degrees[link]; + for (const auto & child : node->children()) { + ++degrees[child]; } } @@ -61,9 +61,9 @@ void topological_sort(std::vector> & input) while (!buffer.empty()) { const auto node = buffer.front(); buffer.pop_front(); - for (const auto & link : node->links()) { - if (--degrees[link] == 0) { - buffer.push_back(link); + for (const auto & child : node->children()) { + if (--degrees[child] == 0) { + buffer.push_back(child); } } result.push_back(node); @@ -71,22 +71,51 @@ void topological_sort(std::vector> & input) // Detect circulation because the result does not include the nodes on the loop. if (result.size() != nodes.size()) { - throw ConfigError("detect graph circulation"); + throw error("detect graph circulation"); } // Reverse the result to process from leaf node. - result = std::deque(result.rbegin(), result.rend()); + result = std::deque(result.rbegin(), result.rend()); // Replace node vector. - std::unordered_map indices; + std::unordered_map indices; for (size_t i = 0; i < result.size(); ++i) { indices[result[i]] = i; } - std::vector> sorted(input.size()); + std::vector> sorted(input.size()); for (auto & node : input) { sorted[indices[node.get()]] = std::move(node); } - input = std::move(sorted); + return sorted; +} + +BaseUnit::UniquePtr make_node(const UnitConfig::SharedPtr & config) +{ + if (config->type == "diag") { + return std::make_unique(config->path); + } + if (config->type == "and") { + return std::make_unique(config->path, false); + } + if (config->type == "short-circuit-and") { + return std::make_unique(config->path, true); + } + if (config->type == "or") { + return std::make_unique(config->path); + } + if (config->type == "ok") { + return std::make_unique(config->path, DiagnosticStatus::OK); + } + if (config->type == "warn") { + return std::make_unique(config->path, DiagnosticStatus::WARN); + } + if (config->type == "error") { + return std::make_unique(config->path, DiagnosticStatus::ERROR); + } + if (config->type == "stale") { + return std::make_unique(config->path, DiagnosticStatus::STALE); + } + throw error("unknown node type", config->type, config->data); } Graph::Graph() @@ -101,99 +130,80 @@ Graph::~Graph() void Graph::init(const std::string & file, const std::string & mode) { - ConfigFile root = load_config_root(file); + (void)mode; // TODO(Takagi, Isamu) - std::vector> nodes; - std::unordered_map diags; - std::unordered_map paths; - ExprInit exprs(mode); + BaseUnit::UniquePtrList nodes; + BaseUnit::NodeDict dict; - for (auto & config : root.units) { - if (config.mode.check(mode)) { - auto node = std::make_unique(config.path); - nodes.push_back(std::move(node)); - } - } - for (auto & config : root.diags) { - if (config.mode.check(mode)) { - auto node = std::make_unique(config.path, config.dict); - diags[node->name()] = node.get(); - nodes.push_back(std::move(node)); - } + for (const auto & config : load_root_config(file).nodes) { + const auto node = nodes.emplace_back(make_node(config)).get(); + dict.configs[config] = node; + dict.paths[config->path] = node; } + dict.paths.erase(""); - // TODO(Takagi, Isamu): unknown diag names - { - auto node = std::make_unique("/unknown"); - unknown_ = node.get(); - nodes.push_back(std::move(node)); + for (const auto & [config, node] : dict.configs) { + node->init(config, dict); } - for (const auto & node : nodes) { - paths[node->path()] = node.get(); - } + // Sort units in topological order for update dependencies. + nodes = topological_sort(std::move(nodes)); - for (auto & config : root.units) { - if (paths.count(config.path)) { - paths.at(config.path)->create(config.dict, exprs); - } - } - for (auto & config : root.diags) { - if (paths.count(config.path)) { - paths.at(config.path)->create(config.dict, exprs); - } + for (size_t index = 0; index < nodes.size(); ++index) { + nodes[index]->set_index(index); } - for (auto & [link, config] : exprs.get()) { - link->init(config, paths); - } - - // Sort unit nodes in topological order for update dependencies. - topological_sort(nodes); - - // Set the link index for the ros message. - for (size_t i = 0; i < nodes.size(); ++i) { - nodes[i]->set_index(i); + for (const auto & node : nodes) { + const auto diag = dynamic_cast(node.get()); + if (diag) { + diags_[diag->name()] = diag; + std::cout << diag->name() << std::endl; + } } - nodes_ = std::move(nodes); - diags_ = diags; } -void Graph::callback(const DiagnosticArray & array, const rclcpp::Time & stamp) +void Graph::callback(const rclcpp::Time & stamp, const DiagnosticArray & array) { for (const auto & status : array.status) { const auto iter = diags_.find(status.name); if (iter != diags_.end()) { - iter->second->callback(status, stamp); + iter->second->callback(stamp, status); } else { - unknown_->callback(status, stamp); + // TODO(Takagi, Isamu) + std::cout << "unknown diag: " << status.name << std::endl; } } } -void Graph::update(const rclcpp::Time & stamp) +DiagnosticGraph Graph::report(const rclcpp::Time & stamp) { for (const auto & node : nodes_) { node->update(stamp); } - stamp_ = stamp; -} -DiagnosticGraph Graph::message() const -{ - DiagnosticGraph result; - result.stamp = stamp_; - result.nodes.reserve(nodes_.size()); + DiagnosticGraph message; + message.stamp = stamp; + message.nodes.reserve(nodes_.size()); for (const auto & node : nodes_) { - result.nodes.push_back(node->report()); + const auto report = node->report(); + DiagnosticNode temp; + temp.status.name = node->path(); + temp.status.level = report.level; + for (const auto & [ref, used] : report.links) { + DiagnosticLink link; + link.index = ref->index(); + link.used = used; + temp.links.push_back(link); + } + message.nodes.push_back(temp); } - return result; + return message; } -std::vector Graph::nodes() const +std::vector Graph::nodes() const { - std::vector result; + std::vector result; result.reserve(nodes_.size()); for (const auto & node : nodes_) { result.push_back(node.get()); diff --git a/system/system_diagnostic_graph/src/core/graph.hpp b/system/system_diagnostic_graph/src/core/graph.hpp index e0060c9111592..366f6b457e272 100644 --- a/system/system_diagnostic_graph/src/core/graph.hpp +++ b/system/system_diagnostic_graph/src/core/graph.hpp @@ -34,19 +34,16 @@ class Graph final Graph(); ~Graph(); - void init(const std::string & file, const std::string & mode); - void callback(const DiagnosticArray & array, const rclcpp::Time & stamp); - void update(const rclcpp::Time & stamp); - DiagnosticGraph message() const; - std::vector nodes() const; + void init(const std::string & file, const std::string & mode = ""); + void callback(const rclcpp::Time & stamp, const DiagnosticArray & array); + DiagnosticGraph report(const rclcpp::Time & stamp); + std::vector nodes() const; void debug(); private: - std::vector> nodes_; - std::unordered_map diags_; - UnknownNode * unknown_; - rclcpp::Time stamp_; + std::vector> nodes_; + std::unordered_map diags_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/modes.cpp b/system/system_diagnostic_graph/src/core/modes.cpp index 0ca18f84b0407..96944bd50f81a 100644 --- a/system/system_diagnostic_graph/src/core/modes.cpp +++ b/system/system_diagnostic_graph/src/core/modes.cpp @@ -15,7 +15,8 @@ #include "modes.hpp" #include "config.hpp" -#include "nodes.hpp" +#include "error.hpp" +#include "units.hpp" #include #include @@ -24,22 +25,22 @@ namespace system_diagnostic_graph { -OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) +OperationModes::OperationModes(rclcpp::Node & node, const std::vector & graph) { pub_ = node.create_publisher("/system/operation_mode/availability", rclcpp::QoS(1)); - using PathNodes = std::unordered_map; - PathNodes paths; + using PathDict = std::unordered_map; + PathDict paths; for (const auto & node : graph) { paths[node->path()] = node; } - const auto find_node = [](const PathNodes & paths, const std::string & name) { + const auto find_node = [](const PathDict & paths, const std::string & name) { const auto iter = paths.find(name); if (iter != paths.end()) { return iter->second; } - throw ConfigError("summary node '" + name + "' does node exist"); + throw error("summary node is not found", name); }; // clang-format off @@ -55,7 +56,7 @@ OperationModes::OperationModes(rclcpp::Node & node, const std::vectorlevel() == DiagnosticStatus::OK; }; + const auto is_ok = [](const BaseUnit * node) { return node->level() == DiagnosticStatus::OK; }; // clang-format off Availability message; diff --git a/system/system_diagnostic_graph/src/core/modes.hpp b/system/system_diagnostic_graph/src/core/modes.hpp index ead1ec10dce93..47a31b8d2a152 100644 --- a/system/system_diagnostic_graph/src/core/modes.hpp +++ b/system/system_diagnostic_graph/src/core/modes.hpp @@ -29,7 +29,7 @@ namespace system_diagnostic_graph class OperationModes { public: - explicit OperationModes(rclcpp::Node & node, const std::vector & graph); + OperationModes(rclcpp::Node & node, const std::vector & graph); void update(const rclcpp::Time & stamp) const; private: @@ -37,13 +37,13 @@ class OperationModes rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr pub_; - BaseNode * stop_mode_; - BaseNode * autonomous_mode_; - BaseNode * local_mode_; - BaseNode * remote_mode_; - BaseNode * emergency_stop_mrm_; - BaseNode * comfortable_stop_mrm_; - BaseNode * pull_over_mrm_; + BaseUnit * stop_mode_; + BaseUnit * autonomous_mode_; + BaseUnit * local_mode_; + BaseUnit * remote_mode_; + BaseUnit * emergency_stop_mrm_; + BaseUnit * comfortable_stop_mrm_; + BaseUnit * pull_over_mrm_; }; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.cpp b/system/system_diagnostic_graph/src/core/nodes.cpp deleted file mode 100644 index bbc4bb4d42561..0000000000000 --- a/system/system_diagnostic_graph/src/core/nodes.cpp +++ /dev/null @@ -1,157 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include "nodes.hpp" - -#include "exprs.hpp" - -#include - -#include -#include - -namespace system_diagnostic_graph -{ - -BaseNode::BaseNode(const std::string & path) : path_(path) -{ - index_ = 0; -} - -UnitNode::UnitNode(const std::string & path) : BaseNode(path) -{ - level_ = DiagnosticStatus::STALE; -} - -UnitNode::~UnitNode() -{ - // for unique_ptr -} - -void UnitNode::create(ConfigObject & config, ExprInit & exprs) -{ - expr_ = exprs.create(parse_expr_config(config)); -} - -void UnitNode::update(const rclcpp::Time &) -{ - const auto result = expr_->eval(); - level_ = result.level; - links_.clear(); - for (const auto & [node, used] : result.links) { - DiagnosticLink link; - link.index = node->index(); - link.used = used; - links_.push_back(link); - } -} - -DiagnosticNode UnitNode::report() const -{ - DiagnosticNode message; - message.status.level = level_; - message.status.name = path_; - message.links = links_; - return message; -} - -DiagnosticLevel UnitNode::level() const -{ - return level_; -} - -std::vector UnitNode::links() const -{ - return expr_->get_dependency(); -} - -DiagNode::DiagNode(const std::string & path, ConfigObject & config) : BaseNode(path) -{ - timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize - name_ = config.take_text("name"); - - status_.level = DiagnosticStatus::STALE; -} - -void DiagNode::create(ConfigObject &, ExprInit &) -{ -} - -void DiagNode::update(const rclcpp::Time & stamp) -{ - if (time_) { - const auto elapsed = (stamp - time_.value()).seconds(); - if (timeout_ < elapsed) { - status_ = DiagnosticStatus(); - status_.level = DiagnosticStatus::STALE; - time_ = std::nullopt; - } - } -} - -DiagnosticNode DiagNode::report() const -{ - DiagnosticNode message; - message.status = status_; - message.status.name = path_; - return message; -} - -DiagnosticLevel DiagNode::level() const -{ - return status_.level; -} - -void DiagNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - status_ = status; - time_ = stamp; -} - -UnknownNode::UnknownNode(const std::string & path) : BaseNode(path) -{ -} - -void UnknownNode::create(ConfigObject &, ExprInit &) -{ -} - -void UnknownNode::update(const rclcpp::Time & stamp) -{ - (void)stamp; -} - -DiagnosticNode UnknownNode::report() const -{ - DiagnosticNode message; - message.status.name = path_; - for (const auto & diag : diagnostics_) { - diagnostic_msgs::msg::KeyValue kv; - kv.key = diag.first; - message.status.values.push_back(kv); - } - return message; -} - -DiagnosticLevel UnknownNode::level() const -{ - return DiagnosticStatus::WARN; -} - -void UnknownNode::callback(const DiagnosticStatus & status, const rclcpp::Time & stamp) -{ - diagnostics_[status.name] = stamp; -} - -} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/nodes.hpp b/system/system_diagnostic_graph/src/core/nodes.hpp deleted file mode 100644 index 1a849cf58b268..0000000000000 --- a/system/system_diagnostic_graph/src/core/nodes.hpp +++ /dev/null @@ -1,114 +0,0 @@ -// Copyright 2023 The Autoware Contributors -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef CORE__NODES_HPP_ -#define CORE__NODES_HPP_ - -#include "config.hpp" -#include "debug.hpp" -#include "types.hpp" - -#include - -#include -#include -#include -#include -#include -#include - -namespace system_diagnostic_graph -{ - -class BaseNode -{ -public: - explicit BaseNode(const std::string & path); - virtual ~BaseNode() = default; - virtual void create(ConfigObject & config, ExprInit & exprs) = 0; - virtual void update(const rclcpp::Time & stamp) = 0; - virtual DiagnosticNode report() const = 0; - virtual DiagnosticLevel level() const = 0; - virtual DiagDebugData debug() const = 0; - virtual std::vector links() const = 0; - - std::string path() const { return path_; } - - size_t index() const { return index_; } - void set_index(const size_t index) { index_ = index; } - -protected: - const std::string path_; - size_t index_; -}; - -class UnitNode : public BaseNode -{ -public: - explicit UnitNode(const std::string & path); - ~UnitNode() override; - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override; - -private: - std::unique_ptr expr_; - std::vector links_; - DiagnosticLevel level_; -}; - -class DiagNode : public BaseNode -{ -public: - DiagNode(const std::string & path, ConfigObject & config); - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override { return {}; } - std::string name() const { return name_; } - - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); - -private: - double timeout_; - std::optional time_; - std::string name_; - DiagnosticStatus status_; -}; - -class UnknownNode : public BaseNode -{ -public: - explicit UnknownNode(const std::string & path); - void create(ConfigObject & config, ExprInit & exprs) override; - void update(const rclcpp::Time & stamp) override; - DiagnosticNode report() const override; - DiagnosticLevel level() const override; - DiagDebugData debug() const override; - std::vector links() const override { return {}; } - - void callback(const DiagnosticStatus & status, const rclcpp::Time & stamp); - -private: - std::map diagnostics_; -}; - -} // namespace system_diagnostic_graph - -#endif // CORE__NODES_HPP_ diff --git a/system/system_diagnostic_graph/src/core/types.hpp b/system/system_diagnostic_graph/src/core/types.hpp index 2adfbb3f9d4ef..c0b901d1e4511 100644 --- a/system/system_diagnostic_graph/src/core/types.hpp +++ b/system/system_diagnostic_graph/src/core/types.hpp @@ -31,13 +31,8 @@ using tier4_system_msgs::msg::DiagnosticLink; using tier4_system_msgs::msg::DiagnosticNode; using DiagnosticLevel = DiagnosticStatus::_level_type; -class BaseNode; -class UnitNode; -class DiagNode; -class UnknownNode; - -class BaseExpr; -class ExprInit; +class BaseUnit; +class DiagUnit; } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.cpp b/system/system_diagnostic_graph/src/core/units.cpp new file mode 100644 index 0000000000000..7cdd7c3e1266e --- /dev/null +++ b/system/system_diagnostic_graph/src/core/units.cpp @@ -0,0 +1,164 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "units.hpp" + +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +using LinkList = std::vector>; + +void merge(LinkList & a, const LinkList & b, bool uses) +{ + for (const auto & [node, used] : b) { + a.push_back(std::make_pair(node, used && uses)); + } +} + +auto resolve(const BaseUnit::NodeDict & dict, const std::vector & children) +{ + std::vector result; + for (const auto & child : children) { + result.push_back(dict.configs.at(child)); + } + return result; +} + +auto resolve(const BaseUnit::NodeDict & dict, const std::string & path) +{ + std::vector result; + result.push_back(dict.paths.at(path)); + return result; +} + +BaseUnit::BaseUnit(const std::string & path) : path_(path) +{ + index_ = 0; + level_ = DiagnosticStatus::OK; +} + +BaseUnit::NodeData BaseUnit::status() const +{ + if (path_.empty()) { + return {level_, links_}; + } else { + return {level_, {std::make_pair(this, true)}}; + } +} + +BaseUnit::NodeData BaseUnit::report() const +{ + return {level_, links_}; +} + +void DiagUnit::init(const UnitConfig::SharedPtr & config, const NodeDict &) +{ + timeout_ = 3.0; // TODO(Takagi, Isamu): parameterize + name_ = config->data.take_text("diag"); +} + +void DiagUnit::update(const rclcpp::Time & stamp) +{ + if (diagnostics_) { + const auto updated = diagnostics_.value().first; + const auto elapsed = (stamp - updated).seconds(); + if (timeout_ < elapsed) { + diagnostics_ = std::nullopt; + } + } + + if (diagnostics_) { + level_ = diagnostics_.value().second.level; + } else { + level_ = DiagnosticStatus::STALE; + } +} + +void DiagUnit::callback(const rclcpp::Time & stamp, const DiagnosticStatus & status) +{ + diagnostics_ = std::make_pair(stamp, status); +} + +AndUnit::AndUnit(const std::string & path, bool short_circuit) : BaseUnit(path) +{ + short_circuit_ = short_circuit; +} + +void AndUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->children); +} + +void AndUnit::update(const rclcpp::Time &) +{ + if (children_.empty()) { + return; + } + + bool uses = true; + level_ = DiagnosticStatus::OK; + links_ = LinkList(); + + for (const auto & child : children_) { + const auto status = child->status(); + level_ = std::max(level_, status.level); + merge(links_, status.links, uses); + if (short_circuit_ && level_ != DiagnosticStatus::OK) { + uses = false; + } + } + level_ = std::min(level_, DiagnosticStatus::ERROR); +} + +void OrUnit::init(const UnitConfig::SharedPtr & config, const NodeDict & dict) +{ + children_ = resolve(dict, config->children); +} + +void OrUnit::update(const rclcpp::Time &) +{ + if (children_.empty()) { + return; + } + + level_ = DiagnosticStatus::ERROR; + links_ = LinkList(); + + for (const auto & child : children_) { + const auto status = child->status(); + level_ = std::min(level_, status.level); + merge(links_, status.links, true); + } + level_ = std::min(level_, DiagnosticStatus::ERROR); +} + +DebugUnit::DebugUnit(const std::string & path, DiagnosticLevel level) : BaseUnit(path) +{ + level_ = level; // overwrite +} + +void DebugUnit::init(const UnitConfig::SharedPtr &, const NodeDict &) +{ +} + +void DebugUnit::update(const rclcpp::Time &) +{ +} + +} // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/src/core/units.hpp b/system/system_diagnostic_graph/src/core/units.hpp new file mode 100644 index 0000000000000..ad5fa4c4bc090 --- /dev/null +++ b/system/system_diagnostic_graph/src/core/units.hpp @@ -0,0 +1,119 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef CORE__UNITS_HPP_ +#define CORE__UNITS_HPP_ + +#include "config.hpp" +#include "types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace system_diagnostic_graph +{ + +class BaseUnit +{ +public: + struct NodeDict + { + std::unordered_map configs; + std::unordered_map paths; + }; + struct NodeData + { + DiagnosticLevel level; + std::vector> links; + }; + using UniquePtr = std::unique_ptr; + using UniquePtrList = std::vector>; + + explicit BaseUnit(const std::string & path); + virtual ~BaseUnit() = default; + virtual void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) = 0; + virtual void update(const rclcpp::Time & stamp) = 0; + + NodeData status() const; + NodeData report() const; + DiagnosticLevel level() const { return level_; } + + auto path() const { return path_; } + auto children() const { return children_; } + + size_t index() const { return index_; } + void set_index(const size_t index) { index_ = index; } + +protected: + DiagnosticLevel level_; + std::string path_; + std::vector children_; + std::vector> links_; + +private: + size_t index_; +}; + +class DiagUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; + + std::string name() const { return name_; } + void callback(const rclcpp::Time & stamp, const DiagnosticStatus & status); + +private: + double timeout_; + std::optional> diagnostics_; + std::string name_; +}; + +class AndUnit : public BaseUnit +{ +public: + AndUnit(const std::string & path, bool short_circuit); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; + +private: + bool short_circuit_; +}; + +class OrUnit : public BaseUnit +{ +public: + using BaseUnit::BaseUnit; + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; +}; + +class DebugUnit : public BaseUnit +{ +public: + DebugUnit(const std::string & path, DiagnosticLevel level); + void init(const UnitConfig::SharedPtr & config, const NodeDict & dict) override; + void update(const rclcpp::Time & stamp) override; +}; + +} // namespace system_diagnostic_graph + +#endif // CORE__UNITS_HPP_ diff --git a/system/system_diagnostic_graph/src/main.cpp b/system/system_diagnostic_graph/src/main.cpp index 722ddcf833577..7db35d1fd2551 100644 --- a/system/system_diagnostic_graph/src/main.cpp +++ b/system/system_diagnostic_graph/src/main.cpp @@ -58,16 +58,14 @@ MainNode::~MainNode() void MainNode::on_timer() { const auto stamp = now(); - graph_.update(stamp); + pub_graph_->publish(graph_.report(stamp)); graph_.debug(); - pub_graph_->publish(graph_.message()); - if (modes_) modes_->update(stamp); } void MainNode::on_diag(const DiagnosticArray::ConstSharedPtr msg) { - graph_.callback(*msg, now()); + graph_.callback(now(), *msg); } } // namespace system_diagnostic_graph diff --git a/system/system_diagnostic_graph/test/files/field-not-found.yaml b/system/system_diagnostic_graph/test/files/field-not-found.yaml new file mode 100644 index 0000000000000..9e2b3708c049a --- /dev/null +++ b/system/system_diagnostic_graph/test/files/field-not-found.yaml @@ -0,0 +1,2 @@ +nodes: + - test-type: test-type diff --git a/system/system_diagnostic_graph/test/files/file-not-found.yaml b/system/system_diagnostic_graph/test/files/file-not-found.yaml new file mode 100644 index 0000000000000..ffc83b42096f0 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/file-not-found.yaml @@ -0,0 +1,2 @@ +files: + - { path: $(dirname)/fake-file-name.yaml } diff --git a/system/system_diagnostic_graph/test/files/graph-circulation.yaml b/system/system_diagnostic_graph/test/files/graph-circulation.yaml new file mode 100644 index 0000000000000..4c014bdcec3f9 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/graph-circulation.yaml @@ -0,0 +1,9 @@ +nodes: + - path: /foo/bar + type: and + list: + - { type: link, link: /foo/baz } + - path: /foo/baz + type: and + list: + - { type: link, link: /foo/bar } diff --git a/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml b/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml new file mode 100644 index 0000000000000..6499b05ab8b7d --- /dev/null +++ b/system/system_diagnostic_graph/test/files/invalid-dict-type.yaml @@ -0,0 +1 @@ +nodes: test-object diff --git a/system/system_diagnostic_graph/test/files/invalid-list-type.yaml b/system/system_diagnostic_graph/test/files/invalid-list-type.yaml new file mode 100644 index 0000000000000..4fc5d96c08c62 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/invalid-list-type.yaml @@ -0,0 +1,3 @@ +nodes: + - type: and + list: test-list diff --git a/system/system_diagnostic_graph/test/files/path-conflict.yaml b/system/system_diagnostic_graph/test/files/path-conflict.yaml new file mode 100644 index 0000000000000..ea3e536b74216 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/path-conflict.yaml @@ -0,0 +1,5 @@ +nodes: + - path: /foo/bar + type: and + - path: /foo/bar + type: and diff --git a/system/system_diagnostic_graph/test/files/path-not-found.yaml b/system/system_diagnostic_graph/test/files/path-not-found.yaml new file mode 100644 index 0000000000000..6b0b10dfa94f4 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/path-not-found.yaml @@ -0,0 +1,5 @@ +nodes: + - path: /foo/bar + type: and + - link: /foo/baz + type: link diff --git a/system/system_diagnostic_graph/test/files/unknown-node-type.yaml b/system/system_diagnostic_graph/test/files/unknown-node-type.yaml new file mode 100644 index 0000000000000..a3d66fbfa43cb --- /dev/null +++ b/system/system_diagnostic_graph/test/files/unknown-node-type.yaml @@ -0,0 +1,2 @@ +nodes: + - type: test-type diff --git a/system/system_diagnostic_graph/test/files/unknown-substitution.yaml b/system/system_diagnostic_graph/test/files/unknown-substitution.yaml new file mode 100644 index 0000000000000..7286321c30439 --- /dev/null +++ b/system/system_diagnostic_graph/test/files/unknown-substitution.yaml @@ -0,0 +1,2 @@ +files: + - { path: $(dummy) } diff --git a/system/system_diagnostic_graph/test/src/test.cpp b/system/system_diagnostic_graph/test/src/test.cpp new file mode 100644 index 0000000000000..b763179be0791 --- /dev/null +++ b/system/system_diagnostic_graph/test/src/test.cpp @@ -0,0 +1,88 @@ +// Copyright 2023 The Autoware Contributors +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "core/error.hpp" +#include "core/graph.hpp" + +#include + +#include +#include + +using namespace system_diagnostic_graph; // NOLINT(build/namespaces) + +std::filesystem::path resource(const std::string & path) +{ + return std::filesystem::path(TEST_RESOURCE_PATH) / path; +} + +TEST(ConfigFile, RootNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("fake-file-name.yaml")), FileNotFound); +} + +TEST(ConfigFile, FileNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("file-not-found.yaml")), FileNotFound); +} + +TEST(ConfigFile, UnknownSubstitution) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("unknown-substitution.yaml")), UnknownType); +} + +TEST(ConfigFile, UnknownNodeType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("unknown-node-type.yaml")), UnknownType); +} + +TEST(ConfigFile, InvalidDictType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("invalid-dict-type.yaml")), InvalidType); +} + +TEST(ConfigFile, InvalidListType) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("invalid-list-type.yaml")), InvalidType); +} + +TEST(ConfigFile, FieldNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("field-not-found.yaml")), FieldNotFound); +} + +TEST(ConfigFile, PathConflict) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("path-conflict.yaml")), PathConflict); +} + +TEST(ConfigFile, PathNotFound) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("path-not-found.yaml")), PathNotFound); +} + +TEST(ConfigFile, GraphCirculation) +{ + Graph graph; + EXPECT_THROW(graph.init(resource("graph-circulation.yaml")), GraphStructure); +} diff --git a/system/system_diagnostic_graph/util/debug.py b/system/system_diagnostic_graph/util/debug.py deleted file mode 100755 index dac917a26a2f6..0000000000000 --- a/system/system_diagnostic_graph/util/debug.py +++ /dev/null @@ -1,85 +0,0 @@ -#!/usr/bin/env python3 - -# Copyright 2023 The Autoware Contributors -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -import rclpy -from rclpy.node import Node -from rclpy.qos import QoSDurabilityPolicy -from rclpy.qos import QoSProfile -from tier4_system_msgs.msg import DiagnosticGraph - - -class StructNode(Node): - def __init__(self): - super().__init__("system_diagnostic_graph_tools_struct") - qos_struct = QoSProfile(depth=1, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL) - self.sub_struct = self.create_subscription( - DiagnosticGraph, "/diagnostics_graph/struct", self.callback, qos_struct - ) - self.message = None - - def callback(self, msg): - self.message = msg - - -class NodeSpinner: - def __init__(self, time): - self.time = time - self.wait = True - - def on_timer(self): - self.wait = False - - def spin(self, node): - timer = node.create_timer(self.time, self.on_timer) - while self.wait: - rclpy.spin_once(node) - node.destroy_timer(timer) - - -class GraphNode: - def __init__(self, msg): - self.name = msg.name - self.links = msg.links - - -class GraphStruct: - def __init__(self, msg): - self.nodes = [GraphNode(node) for node in msg.nodes] - - @staticmethod - def Subscribe(): - spin = NodeSpinner(1.0) - node = StructNode() - spin.spin(node) - return GraphStruct(node.message) - - def visualize(self): - from graphviz import Digraph - - graph = Digraph() - graph.attr("node", shape="box") - for node in self.nodes: - graph.node(node.name) - for link in node.links: - graph.edge(node.name, self.nodes[link].name) - graph.view() - - -if __name__ == "__main__": - rclpy.init() - graph = GraphStruct.Subscribe() - rclpy.shutdown() - graph.visualize()