From 8dae2a24bb8f1a1ef6b38177e71462287b0ff20e Mon Sep 17 00:00:00 2001 From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> Date: Sat, 1 Oct 2022 15:17:16 +0900 Subject: [PATCH] refactor(simple_planning_simulator): refactor covariance index (#1972) Signed-off-by: scepter914 Signed-off-by: scepter914 --- .../simple_planning_simulator_core.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index ad013e1846f8a..95c387ce6bcf5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -19,6 +19,7 @@ #include "motion_common/motion_common.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "tier4_autoware_utils/ros/msg_covariance.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -286,8 +287,9 @@ void SimplePlanningSimulator::on_timer() // add estimate covariance { - current_odometry_.pose.covariance[0 * 6 + 0] = x_stddev_; - current_odometry_.pose.covariance[1 * 6 + 1] = y_stddev_; + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; + current_odometry_.pose.covariance[COV_IDX::X_X] = x_stddev_; + current_odometry_.pose.covariance[COV_IDX::Y_Y] = y_stddev_; } // publish vehicle state @@ -527,13 +529,14 @@ void SimplePlanningSimulator::publish_acceleration() msg.header.stamp = get_clock()->now(); msg.accel.accel.linear.x = vehicle_model_ptr_->getAx(); + using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX; constexpr auto COV = 0.001; - msg.accel.covariance.at(6 * 0 + 0) = COV; // linear x - msg.accel.covariance.at(6 * 1 + 1) = COV; // linear y - msg.accel.covariance.at(6 * 2 + 2) = COV; // linear z - msg.accel.covariance.at(6 * 3 + 3) = COV; // angular x - msg.accel.covariance.at(6 * 4 + 4) = COV; // angular y - msg.accel.covariance.at(6 * 5 + 5) = COV; // angular z + msg.accel.covariance.at(COV_IDX::X_X) = COV; // linear x + msg.accel.covariance.at(COV_IDX::Y_Y) = COV; // linear y + msg.accel.covariance.at(COV_IDX::Z_Z) = COV; // linear z + msg.accel.covariance.at(COV_IDX::ROLL_ROLL) = COV; // angular x + msg.accel.covariance.at(COV_IDX::PITCH_PITCH) = COV; // angular y + msg.accel.covariance.at(COV_IDX::YAW_YAW) = COV; // angular z pub_acc_->publish(msg); }