diff --git a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp index 5f7d08e2acf2b..4ff4765d48c08 100644 --- a/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp +++ b/control/autoware_control_evaluator/include/autoware/control_evaluator/control_evaluator_node.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -44,6 +45,8 @@ using nav_msgs::msg::Odometry; using LaneletMapBin = autoware_auto_mapping_msgs::msg::HADMapBin; using LaneletRoute = autoware_auto_planning_msgs::msg::HADMapRoute; using geometry_msgs::msg::AccelWithCovarianceStamped; +using sensor_msgs::msg::Imu; + /** * @brief Node for control evaluation @@ -62,7 +65,7 @@ class ControlEvaluatorNode : public rclcpp::Node DiagnosticStatus generateAEBDiagnosticStatus(const DiagnosticStatus & diag); DiagnosticStatus generateLaneletDiagnosticStatus(const Pose & ego_pose) const; DiagnosticStatus generateKinematicStateDiagnosticStatus( - const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped); + const Odometry & odom, const Imu & imu); void onDiagnostics(const DiagnosticArray::ConstSharedPtr diag_msg); void onTimer(); @@ -75,8 +78,10 @@ class ControlEvaluatorNode : public rclcpp::Node autoware::universe_utils::InterProcessPollingSubscriber odometry_sub_{ this, "~/input/odometry"}; - autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ - this, "~/input/acceleration"}; + // autoware::universe_utils::InterProcessPollingSubscriber accel_sub_{ + // this, "~/input/acceleration"}; + autoware::universe_utils::InterProcessPollingSubscriber imu_sub_{ + this, "~/input/imu"}; autoware::universe_utils::InterProcessPollingSubscriber traj_sub_{ this, "~/input/trajectory"}; autoware::universe_utils::InterProcessPollingSubscriber< @@ -101,7 +106,7 @@ class ControlEvaluatorNode : public rclcpp::Node route_handler::RouteHandler route_handler_; rclcpp::TimerBase::SharedPtr timer_; - std::optional prev_acc_stamped_{std::nullopt}; + std::optional prev_imu_{std::nullopt}; }; } // namespace control_diagnostics diff --git a/control/autoware_control_evaluator/src/control_evaluator_node.cpp b/control/autoware_control_evaluator/src/control_evaluator_node.cpp index daf5cfa9f9da0..aa939d4e32791 100644 --- a/control/autoware_control_evaluator/src/control_evaluator_node.cpp +++ b/control/autoware_control_evaluator/src/control_evaluator_node.cpp @@ -119,7 +119,7 @@ DiagnosticStatus ControlEvaluatorNode::generateLaneletDiagnosticStatus(const Pos } DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( - const Odometry & odom, const AccelWithCovarianceStamped & accel_stamped) + const Odometry & odom, const Imu & imu) { DiagnosticStatus status; status.name = "kinematic_state"; @@ -129,24 +129,24 @@ DiagnosticStatus ControlEvaluatorNode::generateKinematicStateDiagnosticStatus( key_value.value = std::to_string(odom.twist.twist.linear.x); status.values.push_back(key_value); key_value.key = "acc"; - const auto & acc = accel_stamped.accel.accel.linear.x; + const auto & acc = imu.linear_acceleration.x; key_value.value = std::to_string(acc); status.values.push_back(key_value); key_value.key = "jerk"; const auto jerk = [&]() { - if (!prev_acc_stamped_.has_value()) { - prev_acc_stamped_ = accel_stamped; + if (!prev_imu_.has_value()) { + prev_imu_ = imu; return 0.0; } - const auto t = static_cast(accel_stamped.header.stamp.sec) + - static_cast(accel_stamped.header.stamp.nanosec) * 1e-9; - const auto prev_t = static_cast(prev_acc_stamped_.value().header.stamp.sec) + - static_cast(prev_acc_stamped_.value().header.stamp.nanosec) * 1e-9; + const auto t = static_cast(imu.header.stamp.sec) + + static_cast(imu.header.stamp.nanosec) * 1e-9; + const auto prev_t = static_cast(prev_imu_.value().header.stamp.sec) + + static_cast(prev_imu_.value().header.stamp.nanosec) * 1e-9; const auto dt = t - prev_t; if (dt < std::numeric_limits::epsilon()) return 0.0; - const auto prev_acc = prev_acc_stamped_.value().accel.accel.linear.x; - prev_acc_stamped_ = accel_stamped; + const auto prev_acc = prev_imu_.value().linear_acceleration.x; + prev_imu_ = imu; return (acc - prev_acc) / dt; }(); key_value.value = std::to_string(jerk); @@ -191,7 +191,8 @@ void ControlEvaluatorNode::onTimer() DiagnosticArray metrics_msg; const auto traj = traj_sub_.takeData(); const auto odom = odometry_sub_.takeData(); - const auto acc = accel_sub_.takeData(); + // const auto acc = accel_sub_.takeData(); + const auto imu = imu_sub_.takeData(); // generate decision diagnostics from input diagnostics for (const auto & function : target_functions_) { @@ -224,8 +225,8 @@ void ControlEvaluatorNode::onTimer() metrics_msg.status.push_back(generateLaneletDiagnosticStatus(ego_pose)); } - if (odom && acc) { - metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *acc)); + if (odom && imu) { + metrics_msg.status.push_back(generateKinematicStateDiagnosticStatus(*odom, *imu)); } metrics_msg.header.stamp = now();