diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 6b74680a99652..278ef30c4964c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -35,8 +35,9 @@ repos: - id: yamllint - repo: https://github.com/tier4/pre-commit-hooks-ros - rev: v0.6.0 + rev: v0.8.0 hooks: + - id: flake8-ros - id: prettier-xacro - id: prettier-launch-xml - id: prettier-package-xml @@ -65,22 +66,6 @@ repos: - id: black args: [--line-length=100] - - repo: https://github.com/PyCQA/flake8 - rev: 4.0.1 - hooks: - - id: flake8 - additional_dependencies: - [ - flake8-blind-except, - flake8-builtins, - flake8-class-newline, - flake8-comprehensions, - flake8-deprecated, - flake8-docstrings, - flake8-import-order, - flake8-quotes, - ] - - repo: https://github.com/pre-commit/mirrors-clang-format rev: v13.0.1 hooks: diff --git a/common/tier4_debug_tools/package.xml b/common/tier4_debug_tools/package.xml index fa447301d36c3..7387ef6bb1e9f 100644 --- a/common/tier4_debug_tools/package.xml +++ b/common/tier4_debug_tools/package.xml @@ -17,8 +17,8 @@ tier4_debug_msgs launch_ros - rclpy python3-rtree + rclpy ament_lint_auto autoware_lint_common diff --git a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp index 7d0f0d2f7a7d3..0f30805968268 100644 --- a/perception/elevation_map_loader/src/elevation_map_loader_node.cpp +++ b/perception/elevation_map_loader/src/elevation_map_loader_node.cpp @@ -102,7 +102,8 @@ void ElevationMapLoaderNode::publish() try { is_bag_loaded = grid_map::GridMapRosConverter::loadFromBag( *data_manager_.elevation_map_path_, "elevation_map", elevation_map_); - } catch (rosbag2_storage_plugins::SqliteException & e) { + } catch (const std::runtime_error & e) { + RCLCPP_ERROR(this->get_logger(), e.what()); is_bag_loaded = false; } if (!is_bag_loaded) { diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 84f384cf3b726..1ae670216938b 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -26,29 +26,6 @@ #include #include -namespace tier4_autoware_utils -{ -template -double calcLateralOffset( - const T & points, const geometry_msgs::msg::Point & p_target, const size_t seg_idx) -{ - validateNonEmpty(points); - - const auto p_front = getPoint(points.at(seg_idx)); - const auto p_back = getPoint(points.at(seg_idx + 1)); - - const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0}; - const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0}; - - if (segment_vec.norm() == 0.0) { - throw std::runtime_error("Same points are given."); - } - - const Eigen::Vector3d cross_vec = segment_vec.cross(target_vec); - return cross_vec(2) / segment_vec.norm(); -} -} // namespace tier4_autoware_utils - namespace drivable_area_utils { double quantize(const double val, const double resolution) diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 35b9c9d4ab55b..38f977dfc902f 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -62,7 +62,8 @@ class DetectionAreaModule : public SceneModuleInterface public: DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const size_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock); @@ -106,6 +107,8 @@ class DetectionAreaModule : public SceneModuleInterface // Debug DebugData debug_data_; + + int64_t lane_id_; }; } // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index 8dc92269a4e1d..b508dbf353d6d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -28,22 +28,24 @@ namespace behavior_velocity_planner { namespace { -std::vector getDetectionAreaRegElemsOnPath( +using DetectionAreaWithLaneId = std::pair; + +std::vector getDetectionAreaRegElemsOnPath( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const lanelet::LaneletMapPtr lanelet_map) { - std::vector detection_area_reg_elems; + std::vector detection_areas_with_lane_id; for (const auto & p : path.points) { const auto lane_id = p.lane_ids.at(0); const auto ll = lanelet_map->laneletLayer.get(lane_id); const auto detection_areas = ll.regulatoryElementsAs(); for (const auto & detection_area : detection_areas) { - detection_area_reg_elems.push_back(detection_area); + detection_areas_with_lane_id.push_back(std::make_pair(detection_area, lane_id)); } } - return detection_area_reg_elems; + return detection_areas_with_lane_id; } std::set getDetectionAreaIdSetOnPath( @@ -52,7 +54,7 @@ std::set getDetectionAreaIdSetOnPath( { std::set detection_area_id_set; for (const auto & detection_area : getDetectionAreaRegElemsOnPath(path, lanelet_map)) { - detection_area_id_set.insert(detection_area->id()); + detection_area_id_set.insert(detection_area.first->id()); } return detection_area_id_set; } @@ -72,14 +74,15 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) void DetectionAreaModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { - for (const auto & detection_area : + for (const auto & detection_area_with_lane_id : getDetectionAreaRegElemsOnPath(path, planner_data_->route_handler_->getLaneletMapPtr())) { // Use lanelet_id to unregister module when the route is changed - const auto module_id = detection_area->id(); + const auto module_id = detection_area_with_lane_id.first->id(); + const auto lane_id = detection_area_with_lane_id.second; if (!isModuleRegistered(module_id)) { registerModule(std::make_shared( - module_id, *detection_area, planner_param_, logger_.get_child("detection_area_module"), - clock_)); + module_id, lane_id, *(detection_area_with_lane_id.first), planner_param_, + logger_.get_child("detection_area_module"), clock_)); } } } diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 8d0454d2609dd..1ca450dfb1a08 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -69,9 +69,13 @@ boost::optional getNearestCollisionPoint( } boost::optional findCollisionSegment( - const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line) + const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, + const SearchRangeIndex & search_index) { - for (size_t i = 0; i < path.points.size() - 1; ++i) { + const auto min_search_index = std::max(static_cast(0), search_index.min_idx); + const auto max_search_index = std::min(search_index.max_idx, path.points.size() - 1); + + for (size_t i = min_search_index; i < max_search_index; ++i) { const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point const auto & p2 = path.points.at(i + 1).point.pose.position; // Point after collision point @@ -186,13 +190,15 @@ geometry_msgs::msg::Pose calcTargetPose( } // namespace DetectionAreaModule::DetectionAreaModule( - const int64_t module_id, const lanelet::autoware::DetectionArea & detection_area_reg_elem, + const int64_t module_id, const size_t lane_id, + const lanelet::autoware::DetectionArea & detection_area_reg_elem, const PlannerParam & planner_param, const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock) : SceneModuleInterface(module_id, logger, clock), detection_area_reg_elem_(detection_area_reg_elem), state_(State::GO), - planner_param_(planner_param) + planner_param_(planner_param), + lane_id_(lane_id) { } @@ -351,8 +357,13 @@ bool DetectionAreaModule::isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const { - return tier4_autoware_utils::calcSignedArcLength( - path.points, self_pose.position, line_pose.position) < 0; + const PointWithSearchRangeIndex src_point_with_search_range_index = + planning_utils::findFirstNearSearchRangeIndex(path.points, self_pose.position); + const PointWithSearchRangeIndex dst_point_with_search_range_index = { + line_pose.position, planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_)}; + + return planning_utils::calcSignedArcLengthWithSearchIndex( + path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < 0; } bool DetectionAreaModule::hasEnoughBrakingDistance( @@ -392,8 +403,10 @@ boost::optional DetectionAreaModule::createTargetPoint( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line, const double margin) const { + const auto dst_search_range = planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_); + // Find collision segment - const auto collision_segment = findCollisionSegment(path, stop_line); + const auto collision_segment = findCollisionSegment(path, stop_line, dst_search_range); if (!collision_segment) { // No collision return {}; diff --git a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp index 7ac6918f13ed2..d5a302c3275c6 100644 --- a/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/virtual_traffic_light/manager.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "tier4_autoware_utils/geometry/boost_geometry.hpp" + +#include #include #include @@ -81,11 +84,28 @@ VirtualTrafficLightModuleManager::VirtualTrafficLightModuleManager(rclcpp::Node void VirtualTrafficLightModuleManager::launchNewModules( const autoware_auto_planning_msgs::msg::PathWithLaneId & path) { + tier4_autoware_utils::LineString2d ego_path_linestring; + for (const auto & path_point : path.points) { + ego_path_linestring.push_back( + tier4_autoware_utils::fromMsg(path_point.point.pose.position).to_2d()); + } + for (const auto & m : getRegElemMapOnPath( path, planner_data_->route_handler_->getLaneletMapPtr())) { + const auto stop_line_opt = m.first->getStopLine(); + if (!stop_line_opt) { + RCLCPP_FATAL( + logger_, "No stop line at virtual_traffic_light_reg_elem_id = %ld, please fix the map!", + m.first->id()); + continue; + } + // Use lanelet_id to unregister module when the route is changed const auto module_id = m.second.id(); - if (!isModuleRegistered(module_id)) { + if ( + !isModuleRegistered(module_id) && + boost::geometry::intersects( + ego_path_linestring, lanelet::utils::to2D(stop_line_opt.value()).basicLineString())) { registerModule(std::make_shared( module_id, *m.first, m.second, planner_param_, logger_.get_child("virtual_traffic_light_module"), clock_)); diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp index 5c1036b9782f3..f5f43312ead04 100644 --- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp +++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/node.hpp @@ -53,6 +53,10 @@ template boost::optional lerpPose( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() < 2) { + return {}; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = @@ -94,6 +98,10 @@ template double lerpTwistX( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() < 2) { + return 0.0; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = @@ -116,6 +124,10 @@ template double lerpPoseZ( const T & points, const geometry_msgs::msg::Point & target_pos, const size_t closest_seg_idx) { + if (points.size() < 2) { + return 0.0; + } + constexpr double epsilon = 1e-6; const double closest_to_target_dist = diff --git a/planning/obstacle_avoidance_planner/src/node.cpp b/planning/obstacle_avoidance_planner/src/node.cpp index eb75fcbee9006..c5f642ebb7973 100644 --- a/planning/obstacle_avoidance_planner/src/node.cpp +++ b/planning/obstacle_avoidance_planner/src/node.cpp @@ -1093,6 +1093,9 @@ void ObstacleAvoidancePlanner::calcVelocity( const std::vector & path_points, std::vector & traj_points) const { + if (path_points.size() < 2) { + return; + } for (size_t i = 0; i < traj_points.size(); i++) { const size_t nearest_seg_idx = [&]() { const auto opt_seg_idx = tier4_autoware_utils::findNearestSegmentIndex( diff --git a/planning/obstacle_avoidance_planner/src/utils.cpp b/planning/obstacle_avoidance_planner/src/utils.cpp index 90e9daccb0a89..57eef51c89549 100644 --- a/planning/obstacle_avoidance_planner/src/utils.cpp +++ b/planning/obstacle_avoidance_planner/src/utils.cpp @@ -343,26 +343,29 @@ std::vector interpolate2DTraj const auto monotonic_base_yaw = convertEulerAngleToMonotonic(base_yaw); // spline interpolation - const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); - const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); - const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); - - for (size_t i = 0; i < interpolated_x.size(); i++) { - if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { - return std::vector{}; + std::vector interpolated_points; + try { + const auto interpolated_x = interpolation::slerp(base_s, base_x, new_s); + const auto interpolated_y = interpolation::slerp(base_s, base_y, new_s); + const auto interpolated_yaw = interpolation::slerp(base_s, monotonic_base_yaw, new_s); + + for (size_t i = 0; i < interpolated_x.size(); i++) { + if (std::isnan(interpolated_x[i]) || std::isnan(interpolated_y[i])) { + return std::vector{}; + } } - } - std::vector interpolated_points; - for (size_t i = 0; i < interpolated_x.size(); i++) { - autoware_auto_planning_msgs::msg::TrajectoryPoint point; - point.pose.position.x = interpolated_x[i]; - point.pose.position.y = interpolated_y[i]; - point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( - tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); - interpolated_points.push_back(point); + for (size_t i = 0; i < interpolated_x.size(); i++) { + autoware_auto_planning_msgs::msg::TrajectoryPoint point; + point.pose.position.x = interpolated_x[i]; + point.pose.position.y = interpolated_y[i]; + point.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw( + tier4_autoware_utils::normalizeRadian(interpolated_yaw[i])); + interpolated_points.push_back(point); + } + } catch (const std::invalid_argument & e) { + return std::vector{}; } - return interpolated_points; } diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 324a37bfa9559..44de6e2cdf9cb 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -753,6 +753,10 @@ void ObstacleStopPlannerNode::insertVelocity( const std_msgs::msg::Header & trajectory_header, const VehicleInfo & vehicle_info, const double current_acc, const double current_vel, const StopParam & stop_param) { + if (output.size() < 3) { + return; + } + if (planner_data.stop_require) { // insert stop point const auto traj_end_idx = output.size() - 1; diff --git a/planning/planning_error_monitor/README.md b/planning/planning_error_monitor/README.md index 0890b32da6330..1f4cadb5067c4 100644 --- a/planning/planning_error_monitor/README.md +++ b/planning/planning_error_monitor/README.md @@ -47,7 +47,7 @@ This function checks if the relative angle at point1 generated from point2 and 3 | :------------------------ | :------- | :------------------------------------ | :------------ | | `error_interval` | `double` | Error Interval Distance Threshold [m] | 100.0 | | `error_curvature` | `double` | Error Curvature Threshold | 1.0 | -| `error_sharp_angle` | `double` | Error Sharp Angle Threshold | $\pi$/4 | +| `error_sharp_angle` | `double` | Error Sharp Angle Threshold | 2.0 | | `ignore_too_close_points` | `double` | Ignore Too Close Distance Threshold | 0.005 | ## Visualization diff --git a/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml b/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml index b1b2065324d80..f0f596524d2d6 100644 --- a/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml +++ b/planning/planning_error_monitor/launch/planning_error_monitor.launch.xml @@ -9,7 +9,7 @@ - + diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index ad224028ef3c2..939fc71e50758 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -121,7 +121,7 @@ PathWithLaneId removeOverlappingPoints(const PathWithLaneId & input_path) continue; } - constexpr double min_dist = 0.001; + constexpr double min_dist = 0.1; if ( tier4_autoware_utils::calcDistance3d(filtered_path.points.back().point, pt.point) < min_dist) { diff --git a/sensing/imu_corrector/CMakeLists.txt b/sensing/imu_corrector/CMakeLists.txt index 903a6bcab07c5..79e23af53a373 100644 --- a/sensing/imu_corrector/CMakeLists.txt +++ b/sensing/imu_corrector/CMakeLists.txt @@ -14,19 +14,42 @@ endif() find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() +ament_auto_add_library(gyro_bias_estimation_module SHARED + src/gyro_bias_estimation_module.cpp +) + ament_auto_add_library(imu_corrector_node SHARED src/imu_corrector_core.cpp - include/imu_corrector/imu_corrector_core.hpp ) +ament_auto_add_library(gyro_bias_estimator_node SHARED + src/gyro_bias_estimator.cpp +) + +target_link_libraries(gyro_bias_estimator_node gyro_bias_estimation_module) + rclcpp_components_register_node(imu_corrector_node PLUGIN "imu_corrector::ImuCorrector" EXECUTABLE imu_corrector ) +rclcpp_components_register_node(gyro_bias_estimator_node + PLUGIN "imu_corrector::GyroBiasEstimator" + EXECUTABLE gyro_bias_estimator +) + +function(add_testcase filepath) + get_filename_component(filename ${filepath} NAME) + string(REGEX REPLACE ".cpp" "" test_name ${filename}) + ament_add_gmock(${test_name} ${filepath}) + target_link_libraries("${test_name}" gyro_bias_estimation_module imu_corrector_node gyro_bias_estimator_node) + ament_target_dependencies(${test_name} ${${PROJECT_NAME}_FOUND_BUILD_DEPENDS}) +endfunction() + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + add_testcase(test/test_gyro_bias_estimation_module.cpp) endif() ament_auto_package(INSTALL_TO_SHARE diff --git a/sensing/imu_corrector/README.md b/sensing/imu_corrector/README.md index aedbcbc3c09cd..54b4f70abda5e 100644 --- a/sensing/imu_corrector/README.md +++ b/sensing/imu_corrector/README.md @@ -1,6 +1,6 @@ # imu_corrector -## Purpose +## imu_corrector `imu_corrector_node` is a node that correct imu data. @@ -10,8 +10,6 @@ -## Inputs / Outputs - ### Input | Name | Type | Description | @@ -24,9 +22,7 @@ | --------- | ----------------------- | ------------------ | | `~output` | `sensor_msgs::msg::Imu` | corrected imu data | -## Parameters - -### Core Parameters +### Parameters | Name | Type | Description | | ---------------------------- | ------ | ------------------------------------- | @@ -37,12 +33,33 @@ | `angular_velocity_stddev_yy` | double | pitch rate standard deviation [rad/s] | | `angular_velocity_stddev_zz` | double | yaw rate standard deviation [rad/s] | -## Assumptions / Known limits +## gyro_bias_estimator + +`gyro_bias_validator` is a node that validates the bias of the gyroscope. It subscribes to the `sensor_msgs::msg::Imu` topic and validate if the bias of the gyroscope is within the specified range. -## (Optional) Error detection and handling +Note that the node calculates bias from the gyroscope data by averaging the data only when the vehicle is stopped. -## (Optional) Performance characterization +### Input + +| Name | Type | Description | +| ----------------- | ------------------------------------------------ | ---------------- | +| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data | +| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity | -## (Optional) References/External links +### Output -## (Optional) Future extensions / Unimplemented parts +| Name | Type | Description | +| -------------------- | ------------------------------------ | ----------------------------- | +| `~/output/gyro_bias` | `geometry_msgs::msg::Vector3Stamped` | bias of the gyroscope [rad/s] | + +### Parameters + +| Name | Type | Description | +| --------------------------- | ------ | ----------------------------------------------------------------------------- | +| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] | +| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] | +| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] | +| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] | +| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] | +| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] | +| `data_num_threshold` | int | number of data used to calculate bias | diff --git a/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml new file mode 100644 index 0000000000000..d5868e1df416a --- /dev/null +++ b/sensing/imu_corrector/config/gyro_bias_estimator.param.yaml @@ -0,0 +1,6 @@ +/**: + ros__parameters: + gyro_bias_threshold: 0.0015 # [rad/s] + velocity_threshold: 0.0 # [m/s] + timestamp_threshold: 0.1 # [s] + data_num_threshold: 200 # [num] diff --git a/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml new file mode 100644 index 0000000000000..a25ce5f90ed27 --- /dev/null +++ b/sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/sensing/imu_corrector/package.xml b/sensing/imu_corrector/package.xml index 71fece2622338..5fba088d858fa 100644 --- a/sensing/imu_corrector/package.xml +++ b/sensing/imu_corrector/package.xml @@ -9,10 +9,13 @@ ament_cmake_auto + diagnostic_updater rclcpp rclcpp_components sensor_msgs + ament_cmake_gmock + ament_cmake_gtest ament_lint_auto autoware_lint_common diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..2deb6088f6542 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.cpp @@ -0,0 +1,71 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "gyro_bias_estimation_module.hpp" + +namespace imu_corrector +{ +GyroBiasEstimationModule::GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold) +: velocity_threshold_(velocity_threshold), + timestamp_threshold_(timestamp_threshold), + data_num_threshold_(data_num_threshold), + is_stopped_(false) +{ +} + +void GyroBiasEstimationModule::update_gyro( + const double time, const geometry_msgs::msg::Vector3 & gyro) +{ + if (time - last_velocity_time_ > timestamp_threshold_) { + return; + } + if (!is_stopped_) { + return; + } + gyro_buffer_.push_back(gyro); + if (gyro_buffer_.size() > data_num_threshold_) { + gyro_buffer_.pop_front(); + } +} + +void GyroBiasEstimationModule::update_velocity(const double time, const double velocity) +{ + is_stopped_ = velocity <= velocity_threshold_; + last_velocity_time_ = time; +} + +geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const +{ + if (gyro_buffer_.size() < data_num_threshold_) { + throw std::runtime_error("Bias estimation is not yet ready because of insufficient data."); + } + + geometry_msgs::msg::Vector3 bias; + bias.x = 0.0; + bias.y = 0.0; + bias.z = 0.0; + for (const auto & gyro : gyro_buffer_) { + bias.x += gyro.x; + bias.y += gyro.y; + bias.z += gyro.z; + } + bias.x /= gyro_buffer_.size(); + bias.y /= gyro_buffer_.size(); + bias.z /= gyro_buffer_.size(); + return bias; +} + +} // namespace imu_corrector diff --git a/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp new file mode 100644 index 0000000000000..6ebae942fff5d --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimation_module.hpp @@ -0,0 +1,44 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 GYRO_BIAS_ESTIMATION_MODULE_HPP_ +#define GYRO_BIAS_ESTIMATION_MODULE_HPP_ + +#include + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModule +{ +public: + GyroBiasEstimationModule( + const double velocity_threshold, const double timestamp_threshold, + const size_t data_num_threshold); + geometry_msgs::msg::Vector3 get_bias() const; + void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro); + void update_velocity(const double time, const double velocity); + +private: + const double velocity_threshold_; + const double timestamp_threshold_; + const size_t data_num_threshold_; + bool is_stopped_; + std::deque gyro_buffer_; + + double last_velocity_time_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATION_MODULE_HPP_ diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.cpp b/sensing/imu_corrector/src/gyro_bias_estimator.cpp new file mode 100644 index 0000000000000..7a5bdf73600c9 --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.cpp @@ -0,0 +1,120 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "gyro_bias_estimator.hpp" + +namespace imu_corrector +{ +GyroBiasEstimator::GyroBiasEstimator(const rclcpp::NodeOptions & node_options) +: Node("gyro_bias_validator", node_options), + gyro_bias_threshold_(declare_parameter("gyro_bias_threshold")), + angular_velocity_offset_x_(declare_parameter("angular_velocity_offset_x")), + angular_velocity_offset_y_(declare_parameter("angular_velocity_offset_y")), + angular_velocity_offset_z_(declare_parameter("angular_velocity_offset_z")), + updater_(this), + gyro_bias_(std::nullopt) +{ + updater_.setHardwareID(get_name()); + updater_.add("gyro_bias_validator", this, &GyroBiasEstimator::update_diagnostics); + + const double velocity_threshold = declare_parameter("velocity_threshold"); + const double timestamp_threshold = declare_parameter("timestamp_threshold"); + const size_t data_num_threshold = + static_cast(declare_parameter("data_num_threshold")); + gyro_bias_estimation_module_ = std::make_unique( + velocity_threshold, timestamp_threshold, data_num_threshold); + + imu_sub_ = create_subscription( + "~/input/imu_raw", rclcpp::SensorDataQoS(), + [this](const Imu::ConstSharedPtr msg) { callback_imu(msg); }); + twist_sub_ = create_subscription( + "~/input/twist", rclcpp::SensorDataQoS(), + [this](const TwistWithCovarianceStamped::ConstSharedPtr msg) { callback_twist(msg); }); + + gyro_bias_pub_ = create_publisher("~/output/gyro_bias", rclcpp::SensorDataQoS()); +} + +void GyroBiasEstimator::callback_imu(const Imu::ConstSharedPtr imu_msg_ptr) +{ + // Update gyro data + gyro_bias_estimation_module_->update_gyro( + rclcpp::Time(imu_msg_ptr->header.stamp).seconds(), imu_msg_ptr->angular_velocity); + + // Estimate gyro bias + try { + gyro_bias_ = gyro_bias_estimation_module_->get_bias(); + } catch (const std::runtime_error & e) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *(this->get_clock()), 1000, e.what()); + } + + // Publish results for debugging + if (gyro_bias_ != std::nullopt) { + Vector3Stamped gyro_bias_msg; + gyro_bias_msg.header.stamp = this->now(); + gyro_bias_msg.vector = gyro_bias_.value(); + gyro_bias_pub_->publish(gyro_bias_msg); + } + + // Update diagnostics + updater_.force_update(); +} + +void GyroBiasEstimator::callback_twist( + const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr) +{ + gyro_bias_estimation_module_->update_velocity( + rclcpp::Time(twist_msg_ptr->header.stamp).seconds(), twist_msg_ptr->twist.twist.linear.x); +} + +void GyroBiasEstimator::update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat) +{ + if (gyro_bias_ == std::nullopt) { + stat.add("gyro_bias", "Bias estimation is not yet ready because of insufficient data."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "Not initialized"); + } else { + stat.add("gyro_bias_x_for_imu_corrector", gyro_bias_.value().x); + stat.add("gyro_bias_y_for_imu_corrector", gyro_bias_.value().y); + stat.add("gyro_bias_z_for_imu_corrector", gyro_bias_.value().z); + stat.add("estimated_gyro_bias_x", gyro_bias_.value().x - angular_velocity_offset_x_); + stat.add("estimated_gyro_bias_y", gyro_bias_.value().y - angular_velocity_offset_y_); + stat.add("estimated_gyro_bias_z", gyro_bias_.value().z - angular_velocity_offset_z_); + // Validation + const bool is_bias_small_enough = + std::abs(gyro_bias_.value().x - angular_velocity_offset_x_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().y - angular_velocity_offset_y_) < gyro_bias_threshold_ && + std::abs(gyro_bias_.value().z - angular_velocity_offset_z_) < gyro_bias_threshold_; + + // Update diagnostics + if (is_bias_small_enough) { + stat.add("gyro_bias", "OK"); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::OK, "OK"); + } else { + stat.add( + "gyro_bias", + "Gyro bias may be incorrect. Please calibrate IMU and reflect the result in " + "imu_corrector. You may also use the output of gyro_bias_estimator."); + stat.summary(diagnostic_msgs::msg::DiagnosticStatus::WARN, "WARN"); + RCLCPP_WARN( + get_logger(), "gyro_bias_x: %lf, gyro_bias_y: %lf, gyro_bias_z: %lf", + (gyro_bias_.value().x - angular_velocity_offset_x_), + (gyro_bias_.value().y - angular_velocity_offset_y_), + (gyro_bias_.value().z - angular_velocity_offset_z_)); + } + } +} + +} // namespace imu_corrector + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(imu_corrector::GyroBiasEstimator) diff --git a/sensing/imu_corrector/src/gyro_bias_estimator.hpp b/sensing/imu_corrector/src/gyro_bias_estimator.hpp new file mode 100644 index 0000000000000..37ca1d81dc8fa --- /dev/null +++ b/sensing/imu_corrector/src/gyro_bias_estimator.hpp @@ -0,0 +1,66 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 GYRO_BIAS_ESTIMATOR_HPP_ +#define GYRO_BIAS_ESTIMATOR_HPP_ + +#include "gyro_bias_estimation_module.hpp" + +#include +#include + +#include +#include +#include + +#include +#include +#include + +namespace imu_corrector +{ +class GyroBiasEstimator : public rclcpp::Node +{ +private: + using Imu = sensor_msgs::msg::Imu; + using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped; + using Vector3Stamped = geometry_msgs::msg::Vector3Stamped; + using Vector3 = geometry_msgs::msg::Vector3; + +public: + explicit GyroBiasEstimator(const rclcpp::NodeOptions & node_options); + +private: + void update_diagnostics(diagnostic_updater::DiagnosticStatusWrapper & stat); + void callback_imu(const Imu::ConstSharedPtr imu_msg_ptr); + void callback_twist(const TwistWithCovarianceStamped::ConstSharedPtr twist_msg_ptr); + + rclcpp::Subscription::SharedPtr imu_sub_; + rclcpp::Subscription::SharedPtr twist_sub_; + + rclcpp::Publisher::SharedPtr gyro_bias_pub_; + + std::unique_ptr gyro_bias_estimation_module_; + + const double gyro_bias_threshold_; + const double angular_velocity_offset_x_; + const double angular_velocity_offset_y_; + const double angular_velocity_offset_z_; + + diagnostic_updater::Updater updater_; + + std::optional gyro_bias_; +}; +} // namespace imu_corrector + +#endif // GYRO_BIAS_ESTIMATOR_HPP_ diff --git a/sensing/imu_corrector/src/imu_corrector_core.cpp b/sensing/imu_corrector/src/imu_corrector_core.cpp index c540ab828c32c..491f720db7dd0 100644 --- a/sensing/imu_corrector/src/imu_corrector_core.cpp +++ b/sensing/imu_corrector/src/imu_corrector_core.cpp @@ -12,7 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "imu_corrector/imu_corrector_core.hpp" +#include "imu_corrector_core.hpp" + +#include namespace imu_corrector { diff --git a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp b/sensing/imu_corrector/src/imu_corrector_core.hpp similarity index 89% rename from sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp rename to sensing/imu_corrector/src/imu_corrector_core.hpp index 4e4d154d92aac..30e361fe95b34 100644 --- a/sensing/imu_corrector/include/imu_corrector/imu_corrector_core.hpp +++ b/sensing/imu_corrector/src/imu_corrector_core.hpp @@ -11,8 +11,8 @@ // 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 IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ -#define IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#ifndef IMU_CORRECTOR_CORE_HPP_ +#define IMU_CORRECTOR_CORE_HPP_ #include @@ -42,4 +42,4 @@ class ImuCorrector : public rclcpp::Node }; } // namespace imu_corrector -#endif // IMU_CORRECTOR__IMU_CORRECTOR_CORE_HPP_ +#endif // IMU_CORRECTOR_CORE_HPP_ diff --git a/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp new file mode 100644 index 0000000000000..a0da4d0e24e17 --- /dev/null +++ b/sensing/imu_corrector/test/test_gyro_bias_estimation_module.cpp @@ -0,0 +1,65 @@ +// Copyright 2023 TIER IV, Inc. +// +// 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 "../src/gyro_bias_estimation_module.hpp" + +#include + +namespace imu_corrector +{ +class GyroBiasEstimationModuleTest : public ::testing::Test +{ +protected: + double velocity_threshold = 1.0; + double timestamp_threshold = 5.0; + size_t data_num_threshold = 10; + GyroBiasEstimationModule module = + GyroBiasEstimationModule(velocity_threshold, timestamp_threshold, data_num_threshold); +}; + +TEST_F(GyroBiasEstimationModuleTest, GetBiasEstimationWhenVehicleStopped) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 0.0); // velocity = 0.0 < 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_NEAR(module.get_bias().x, gyro.x, 0.0001); + ASSERT_NEAR(module.get_bias().y, gyro.y, 0.0001); + ASSERT_NEAR(module.get_bias().z, gyro.z, 0.0001); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataException) +{ + ASSERT_THROW(module.get_bias(), std::runtime_error); +} + +TEST_F(GyroBiasEstimationModuleTest, GetInsufficientDataExceptionWhenVehicleMoving) +{ + geometry_msgs::msg::Vector3 gyro; + gyro.x = 0.1; + gyro.y = 0.2; + gyro.z = 0.3; + for (size_t i = 0; i < data_num_threshold + 1; ++i) { + module.update_velocity( + i * 0.1 * timestamp_threshold, 5.0); // velocity = 5.0 > 1.0 = velocity_threshold + module.update_gyro(i * 0.1 * timestamp_threshold, gyro); + } + ASSERT_THROW(module.get_bias(), std::runtime_error); +} +} // namespace imu_corrector diff --git a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml index bfbb6beeaf054..c2da61aec3d0f 100644 --- a/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml +++ b/system/ad_service_state_monitor/config/ad_service_state_monitor.param.yaml @@ -53,7 +53,7 @@ transient_local: True /perception/obstacle_segmentation/pointcloud: - module: "sensing" + module: "perception" timeout: 1.0 warn_rate: 5.0 type: "sensor_msgs/msg/PointCloud2" diff --git a/system/system_error_monitor/config/system_error_monitor.param.yaml b/system/system_error_monitor/config/system_error_monitor.param.yaml index 282c3b919867b..fde96aeeebfb2 100644 --- a/system/system_error_monitor/config/system_error_monitor.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.param.yaml @@ -4,7 +4,7 @@ # lf_at: diag level where it becomes Latent Fault # spf_at: diag level where it becomes Single Point Fault # auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# +# ignore_until_waiting_for_route: Determines whether the system will ignore the module until waiting for route. # Note: # empty-value for sf_at, lf_at and spf_at is "none" # default values are: @@ -12,6 +12,7 @@ # lf_at: "warn" # spf_at: "error" # auto_recovery: "true" +# ignore_until_waiting_for_route: "false" --- /**: ros__parameters: @@ -41,6 +42,15 @@ /autoware/vehicle/node_alive_monitoring: default + manual_control: + /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + + /autoware/vehicle/node_alive_monitoring: default + external_control: /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} diff --git a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml index 271555d1d1dc8..cd968ce615e0b 100644 --- a/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml +++ b/system/system_error_monitor/config/system_error_monitor.planning_simulation.param.yaml @@ -4,7 +4,7 @@ # lf_at: diag level where it becomes Latent Fault # spf_at: diag level where it becomes Single Point Fault # auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# +# ignore_until_waiting_for_route: Determines whether the system will ignore the module until waiting for route. # Note: # empty-value for sf_at, lf_at and spf_at is "none" # default values are: @@ -12,15 +12,16 @@ # lf_at: "warn" # spf_at: "error" # auto_recovery: "true" +# ignore_until_waiting_for_route: "false" --- /**: ros__parameters: required_modules: autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default + /autoware/control/autonomous_driving/node_alive_monitoring: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "true"} /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "false"} /autoware/localization/node_alive_monitoring: default # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } @@ -30,7 +31,7 @@ /autoware/perception/node_alive_monitoring: default - /autoware/planning/node_alive_monitoring: default + /autoware/planning/node_alive_monitoring: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "true"} /autoware/planning/performance_monitoring/trajectory_validation: default /autoware/sensing/node_alive_monitoring: default @@ -41,7 +42,7 @@ /autoware/vehicle/node_alive_monitoring: default - external_control: + manual_control: /autoware/control/control_command_gate/node_alive_monitoring: default /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false"} /autoware/control/external_control/external_command_selector/node_alive_monitoring: default @@ -50,3 +51,13 @@ /autoware/system/emergency_stop_operation: default /autoware/vehicle/node_alive_monitoring: default + + external_control: + /autoware/control/control_command_gate/node_alive_monitoring: default + /autoware/control/autonomous_emergency_braking/performance_monitoring/emergency_stop: { sf_at: "none", lf_at: "warn", spf_at: "error", auto_recovery: "false",ignore_until_waiting_for_route: "false"} + /autoware/control/external_control/external_command_selector/node_alive_monitoring: default + + /autoware/system/node_alive_monitoring: default + /autoware/system/emergency_stop_operation: default + + /autoware/vehicle/node_alive_monitoring: default diff --git a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp index dc54b25d8e54b..f033618b2305d 100644 --- a/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp +++ b/system/system_error_monitor/include/system_error_monitor/system_error_monitor_core.hpp @@ -48,6 +48,7 @@ struct DiagConfig std::string lf_at; std::string spf_at; bool auto_recovery; + bool ignore_until_waiting_for_route; }; using RequiredModules = std::vector; @@ -56,6 +57,7 @@ struct KeyName { static constexpr const char * autonomous_driving = "autonomous_driving"; static constexpr const char * external_control = "external_control"; + static constexpr const char * manual_control = "manual_control"; }; class AutowareErrorMonitor : public rclcpp::Node diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index d6cc771e5dbd2..434009eb69d30 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -208,6 +208,22 @@ int isInNoFaultCondition( return false; } +bool ignoreUntilWaitingForRoute( + const autoware_auto_system_msgs::msg::AutowareState & autoware_state, + const DiagConfig & required_module) +{ + using autoware_auto_system_msgs::msg::AutowareState; + using tier4_control_msgs::msg::GateMode; + + const auto is_in_autonomous_ignore_state = + (autoware_state.state == AutowareState::INITIALIZING) || + (autoware_state.state == AutowareState::WAITING_FOR_ROUTE); + + if (is_in_autonomous_ignore_state && required_module.ignore_until_waiting_for_route) { + return true; + } + return false; +} } // namespace AutowareErrorMonitor::AutowareErrorMonitor() @@ -232,6 +248,7 @@ AutowareErrorMonitor::AutowareErrorMonitor() loadRequiredModules(KeyName::autonomous_driving); loadRequiredModules(KeyName::external_control); + loadRequiredModules(KeyName::manual_control); using std::placeholders::_1; using std::placeholders::_2; @@ -321,11 +338,22 @@ void AutowareErrorMonitor::loadRequiredModules(const std::string & key) std::string auto_recovery_approval_str; this->get_parameter_or(auto_recovery_key, auto_recovery_approval_str, std::string("true")); + const auto ignore_until_waiting_for_route_key = + module_name_with_prefix + std::string(".ignore_until_waiting_for_route"); + std::string ignore_until_waiting_for_route_str; + this->get_parameter_or( + ignore_until_waiting_for_route_key, ignore_until_waiting_for_route_str, std::string("false")); + // Convert auto_recovery_approval_str to bool bool auto_recovery_approval{}; std::istringstream(auto_recovery_approval_str) >> std::boolalpha >> auto_recovery_approval; - required_modules.push_back({param_module, sf_at, lf_at, spf_at, auto_recovery_approval}); + bool ignore_until_waiting_for_route{}; + std::istringstream(ignore_until_waiting_for_route_str) >> std::boolalpha >> + ignore_until_waiting_for_route; + + required_modules.push_back( + {param_module, sf_at, lf_at, spf_at, auto_recovery_approval, ignore_until_waiting_for_route}); } required_modules_map_.insert(std::make_pair(key, required_modules)); @@ -450,8 +478,16 @@ void AutowareErrorMonitor::onTimer() } return; } - + // Heartbeat in AutowareState,diag_array times out during AutowareState INITIALIZING due to high + // processing load,add a disable function to avoid Emergencies in isDataHeartbeatTimeout() in + // AutowareState INITIALIZING. if (isDataHeartbeatTimeout()) { + if ((autoware_state_->state == autoware_auto_system_msgs::msg::AutowareState::INITIALIZING)) { + RCLCPP_WARN_THROTTLE( + get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + "ignore heartbeat timeout in initializing state"); + return; + } updateTimeoutHazardStatus(); publishHazardStatus(hazard_status_); return; @@ -460,6 +496,11 @@ void AutowareErrorMonitor::onTimer() current_mode_ = current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO ? KeyName::autonomous_driving : KeyName::external_control; + if ( + current_gate_mode_->data == tier4_control_msgs::msg::GateMode::AUTO && + control_mode_->mode == autoware_auto_vehicle_msgs::msg::ControlModeReport::MANUAL) { + current_mode_ = KeyName::manual_control; + } updateHazardStatus(); publishHazardStatus(hazard_status_); @@ -487,6 +528,9 @@ uint8_t AutowareErrorMonitor::getHazardLevel( using autoware_auto_system_msgs::msg::HazardStatus; if (isOverLevel(diag_level, required_module.spf_at)) { + if (ignoreUntilWaitingForRoute(*autoware_state_, required_module)) { + return HazardStatus::NO_FAULT; + } return HazardStatus::SINGLE_POINT_FAULT; } if (isOverLevel(diag_level, required_module.lf_at)) { diff --git a/system/system_monitor/config/ntp_monitor.param.yaml b/system/system_monitor/config/ntp_monitor.param.yaml index db54f70d1ce59..5f46821f98aff 100644 --- a/system/system_monitor/config/ntp_monitor.param.yaml +++ b/system/system_monitor/config/ntp_monitor.param.yaml @@ -3,3 +3,4 @@ server: ntp.nict.jp offset_warn: 0.1 offset_error: 5.0 + timeout: 5 # The chronyc execution timeout will trigger a warning when this timer expires. diff --git a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp index f4ee2de666c22..db264d4b7eda2 100644 --- a/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp +++ b/system/system_monitor/include/system_monitor/ntp_monitor/ntp_monitor.hpp @@ -53,6 +53,16 @@ class NTPMonitor : public rclcpp::Node void checkOffset( diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + /** + * @brief Timer callback to execute chronyc command + */ + void onTimer(); + + /** + * @brief Timeout callback function for executing chronyc + */ + void onTimeout(); + /** * @brief function to execute chronyc * @param [out] outOffset offset value of NTP time @@ -71,6 +81,19 @@ class NTPMonitor : public rclcpp::Node float offset_warn_; //!< @brief NTP offset(sec) to generate warning float offset_error_; //!< @brief NTP offset(sec) to generate error + int timeout_; //!< @brief Timeout duration for executing chronyc + + rclcpp::TimerBase::SharedPtr timer_; //!< @brief Timer to execute chronyc command + rclcpp::CallbackGroup::SharedPtr timer_callback_group_; //!< @brief Callback Group + std::mutex mutex_; //!< @brief Mutex for output from chronyc command + std::string error_str_; //!< @brief Error string + std::string pipe2_err_str_; //!< @brief Error string regarding pipe2 function call + float offset_; //!< @brief Offset value of NTP time + std::map tracking_map_; //!< @brief Output of chronyc tracking + double elapsed_ms_; //!< @brief Execution time of chronyc command + rclcpp::TimerBase::SharedPtr timeout_timer_; //!< @brief Timeout for executing chronyc + std::mutex timeout_mutex_; //!< @brief Mutex regarding timeout for executing chronyc + bool timeout_expired_; //!< @brief Timeout for executing chronyc has expired or not /** * @brief NTP offset status messages diff --git a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp index 183f86baa2a08..15702852a0e50 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/diag_task.hpp @@ -24,6 +24,25 @@ #include +/** + * @brief Struct for storing process information + */ +struct ProcessInfo +{ + std::string processId; + std::string userName; + std::string priority; + std::string niceValue; + std::string virtualImage; + std::string residentSize; + std::string sharedMemSize; + std::string processStatus; + std::string cpuUsage; + std::string memoryUsage; + std::string cpuTime; + std::string commandName; +}; + class DiagTask : public diagnostic_updater::DiagnosticTask { public: @@ -46,18 +65,18 @@ class DiagTask : public diagnostic_updater::DiagnosticTask if (level_ != DiagStatus::OK) { stat.add("content", content_); } else { - stat.add("COMMAND", command_); - stat.add("%CPU", cpu_); - stat.add("%MEM", mem_); - stat.add("PID", pid_); - stat.add("USER", user_); - stat.add("PR", pr_); - stat.add("NI", ni_); - stat.add("VIRT", virt_); - stat.add("RES", res_); - stat.add("SHR", shr_); - stat.add("S", s_); - stat.add("TIME+", time_); + stat.add("COMMAND", info_.commandName); + stat.add("%CPU", info_.cpuUsage); + stat.add("%MEM", info_.memoryUsage); + stat.add("PID", info_.processId); + stat.add("USER", info_.userName); + stat.add("PR", info_.priority); + stat.add("NI", info_.niceValue); + stat.add("VIRT", info_.virtualImage); + stat.add("RES", info_.residentSize); + stat.add("SHR", info_.sharedMemSize); + stat.add("S", info_.processStatus); + stat.add("TIME+", info_.cpuTime); } stat.summary(level_, message_); } @@ -85,95 +104,17 @@ class DiagTask : public diagnostic_updater::DiagnosticTask } /** - * @brief set process id - * @param [in] pid process id - */ - void setProcessId(const std::string & pid) { pid_ = pid; } - - /** - * @brief set user name - * @param [in] user user name - */ - void setUserName(const std::string & user) { user_ = user; } - - /** - * @brief set priority - * @param [in] pr priority - */ - void setPriority(const std::string & pr) { pr_ = pr; } - - /** - * @brief set nice value - * @param [in] ni nice value - */ - void setNiceValue(const std::string & ni) { ni_ = ni; } - - /** - * @brief set virtual image - * @param [in] virt virtual image - */ - void setVirtualImage(const std::string & virt) { virt_ = virt; } - - /** - * @brief set resident size - * @param [in] res resident size + * @brief Set process information + * @param [in] info Process information */ - void setResidentSize(const std::string & res) { res_ = res; } - - /** - * @brief set shared mem size - * @param [in] shr shared mem size - */ - void setSharedMemSize(const std::string & shr) { shr_ = shr; } - - /** - * @brief set process status - * @param [in] s process status - */ - void setProcessStatus(const std::string & s) { s_ = s; } - - /** - * @brief set CPU usage - * @param [in] cpu CPU usage - */ - void setCPUUsage(const std::string & cpu) { cpu_ = cpu; } - - /** - * @brief set memory usage - * @param [in] mem memory usage - */ - void setMemoryUsage(const std::string & mem) { mem_ = mem; } - - /** - * @brief set CPU time - * @param [in] time CPU time - */ - void setCPUTime(const std::string & time) { time_ = time; } - - /** - * @brief set Command name/line - * @param [in] command Command name/line - */ - void setCommandName(const std::string & command) { command_ = command; } + void setProcessInformation(const struct ProcessInfo & info) { info_ = info; } private: int level_; //!< @brief Diagnostics error level std::string message_; //!< @brief Diagnostics status message std::string error_command_; //!< @brief Error command std::string content_; //!< @brief Error content - - std::string pid_; //!< @brief Process Id - std::string user_; //!< @brief User Name - std::string pr_; //!< @brief Priority - std::string ni_; //!< @brief Nice value - std::string virt_; //!< @brief Virtual Image (kb) - std::string res_; //!< @brief Resident size (kb) - std::string shr_; //!< @brief Shared Mem size (kb) - std::string s_; //!< @brief Process Status - std::string cpu_; //!< @brief CPU usage - std::string mem_; //!< @brief Memory usage - std::string time_; //!< @brief CPU Time - std::string command_; //!< @brief Command name/line + struct ProcessInfo info_; //!< @brief Struct for storing process information }; #endif // SYSTEM_MONITOR__PROCESS_MONITOR__DIAG_TASK_HPP_ diff --git a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp index e114d58770883..ec2a90c6b27e4 100644 --- a/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp +++ b/system/system_monitor/include/system_monitor/process_monitor/process_monitor.hpp @@ -88,6 +88,14 @@ class ProcessMonitor : public rclcpp::Node */ void getHighMemoryProcesses(const std::string & output); + /** + * @brief get command line from process id + * @param [in] pid process id + * @param [out] command output command line + * @return true if success to get command line name + */ + bool getCommandLineFromPiD(const std::string & pid, std::string & command); + /** * @brief get top-rated processes * @param [in] tasks list of diagnostics tasks for high load procs diff --git a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp index 10dba2f96e164..0b91b28b2b5f9 100644 --- a/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp +++ b/system/system_monitor/src/ntp_monitor/ntp_monitor.cpp @@ -19,7 +19,7 @@ #include "system_monitor/ntp_monitor/ntp_monitor.hpp" -#include "system_monitor/system_monitor_utility.hpp" +#include #include #include @@ -37,8 +37,12 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) : Node("ntp_monitor", options), updater_(this), offset_warn_(declare_parameter("offset_warn", 0.1)), - offset_error_(declare_parameter("offset_error", 5.0)) + offset_error_(declare_parameter("offset_error", 5.0)), + timeout_(declare_parameter("timeout", 5)), + timeout_expired_(false) { + using namespace std::literals::chrono_literals; + gethostname(hostname_, sizeof(hostname_)); // Check if command exists @@ -47,15 +51,15 @@ NTPMonitor::NTPMonitor(const rclcpp::NodeOptions & options) updater_.setHardwareID(hostname_); updater_.add("NTP Offset", this, &NTPMonitor::checkOffset); -} -void NTPMonitor::update() { updater_.force_update(); } + // Start timer to execute top command + timer_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&NTPMonitor::onTimer, this), timer_callback_group_); +} void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) { - // Remember start time to measure elapsed time - const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!chronyc_exists_) { stat.summary(DiagStatus::ERROR, "chronyc error"); stat.add( @@ -67,7 +71,18 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) std::string pipe2_err_str; float offset = 0.0f; std::map tracking_map; - error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + double elapsed_ms; + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str = error_str_; + pipe2_err_str = pipe2_err_str_; + offset = offset_; + tracking_map = tracking_map_; + elapsed_ms = elapsed_ms_; + } + if (!pipe2_err_str.empty()) { stat.summary(DiagStatus::ERROR, "pipe2 error"); stat.add("pipe2", pipe2_err_str); @@ -91,10 +106,65 @@ void NTPMonitor::checkOffset(diagnostic_updater::DiagnosticStatusWrapper & stat) for (auto itr = tracking_map.begin(); itr != tracking_map.end(); ++itr) { stat.add(itr->first, itr->second); } - stat.summary(level, offset_dict_.at(level)); - // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + // Check timeout has expired regarding executing chronyc + bool timeout_expired = false; + { + std::lock_guard lock(timeout_mutex_); + timeout_expired = timeout_expired_; + } + + if (!timeout_expired) { + stat.summary(level, offset_dict_.at(level)); + } else { + stat.summary(DiagStatus::WARN, "chronyc timeout expired"); + } + + stat.addf("execution time", "%f ms", elapsed_ms); +} + +void NTPMonitor::onTimer() +{ + // Start to measure elapsed time + tier4_autoware_utils::StopWatch stop_watch; + stop_watch.tic("execution_time"); + + std::string error_str; + std::string pipe2_err_str; + float offset = 0.0f; + std::map tracking_map; + + // Start timeout timer for executing chronyc + { + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = false; + } + timeout_timer_ = rclcpp::create_timer( + this, get_clock(), std::chrono::seconds(timeout_), std::bind(&NTPMonitor::onTimeout, this)); + + error_str = executeChronyc(offset, tracking_map, pipe2_err_str); + + // Returning from chronyc, stop timeout timer + timeout_timer_->cancel(); + + const double elapsed_ms = stop_watch.toc("execution_time"); + + // thread-safe copy + { + std::lock_guard lock(mutex_); + error_str_ = error_str; + pipe2_err_str_ = pipe2_err_str; + offset_ = offset; + tracking_map_ = tracking_map; + elapsed_ms_ = elapsed_ms; + } +} + +void NTPMonitor::onTimeout() +{ + RCLCPP_WARN(get_logger(), "Timeout occurred."); + std::lock_guard lock(timeout_mutex_); + timeout_expired_ = true; } std::string NTPMonitor::executeChronyc( diff --git a/system/system_monitor/src/process_monitor/process_monitor.cpp b/system/system_monitor/src/process_monitor/process_monitor.cpp index f78fc00c430dc..1173431b0f7c2 100644 --- a/system/system_monitor/src/process_monitor/process_monitor.cpp +++ b/system/system_monitor/src/process_monitor/process_monitor.cpp @@ -401,7 +401,7 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) bp::pipe err_pipe{err_fd[0], err_fd[1]}; bp::ipstream is_err{std::move(err_pipe)}; - bp::child c("sort -r -k 10", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); + bp::child c("sort -r -k 10 -n", bp::std_out > p2, bp::std_err > is_err, bp::std_in < p1); c.wait(); if (c.exit_code() != 0) { is_err >> os.rdbuf(); @@ -414,6 +414,27 @@ void ProcessMonitor::getHighMemoryProcesses(const std::string & output) getTopratedProcesses(&memory_tasks_, &p2); } +bool ProcessMonitor::getCommandLineFromPiD(const std::string & pid, std::string & command) +{ + std::string commandLineFilePath = "/proc/" + pid + "/cmdline"; + std::ifstream commandFile(commandLineFilePath, std::ios::in | std::ios::binary); + + if (commandFile.is_open()) { + std::vector buffer; + std::copy( + std::istream_iterator(commandFile), std::istream_iterator(), + std::back_inserter(buffer)); + commandFile.close(); + std::replace( + buffer.begin(), buffer.end(), '\0', + ' '); // 0x00 is used as delimiter in /cmdline instead of 0x20 (space) + command = std::string(buffer.begin(), buffer.end()); + return (buffer.size() > 0) ? true : false; // cmdline is empty if it is kernel process + } else { + return false; + } +} + void ProcessMonitor::getTopratedProcesses( std::vector> * tasks, bp::pipe * p) { @@ -451,27 +472,28 @@ void ProcessMonitor::getTopratedProcesses( return; } - std::vector list; std::string line; int index = 0; while (std::getline(is_out, line) && !line.empty()) { - boost::trim_left(line); - boost::split(list, line, boost::is_space(), boost::token_compress_on); + std::istringstream stream(line); + + ProcessInfo info; + stream >> info.processId >> info.userName >> info.priority >> info.niceValue >> + info.virtualImage >> info.residentSize >> info.sharedMemSize >> info.processStatus >> + info.cpuUsage >> info.memoryUsage >> info.cpuTime; + + std::string program_name; + std::getline(stream, program_name); + + bool flag_find_command_line = getCommandLineFromPiD(info.processId, info.commandName); + + if (!flag_find_command_line) { + info.commandName = program_name; // if command line is not found, use program name instead + } tasks->at(index)->setDiagnosticsStatus(DiagStatus::OK, "OK"); - tasks->at(index)->setProcessId(list[0]); - tasks->at(index)->setUserName(list[1]); - tasks->at(index)->setPriority(list[2]); - tasks->at(index)->setNiceValue(list[3]); - tasks->at(index)->setVirtualImage(list[4]); - tasks->at(index)->setResidentSize(list[5]); - tasks->at(index)->setSharedMemSize(list[6]); - tasks->at(index)->setProcessStatus(list[7]); - tasks->at(index)->setCPUUsage(list[8]); - tasks->at(index)->setMemoryUsage(list[9]); - tasks->at(index)->setCPUTime(list[10]); - tasks->at(index)->setCommandName(list[11]); + tasks->at(index)->setProcessInformation(info); ++index; } }